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Coordinated Robot Navigation 
via Hierarchical Clustering 

Omur Arslan, Dan P. Guralnik and Daniel E. Koditschek, 


Abstract —We introduce the use of hierarchical clustering 
for relaxed, deterministic coordination and control of multiple 
robots. Traditionally an unsupervised learning method, hierarchi¬ 
cal clustering offers a formalism for identifying and representing 
spatially cohesive and segregated robot groups at different 
resolutions by relating the continuous space of configurations to 
the combinatorial space of trees. We formalize and exploit this 
relation, developing computationally effective reactive algorithms 
for navigating through the combinatorial space in concert with 
geometric realizations for a particular choice of hierarchical 
clustering method. These constructions yield computationally 
effective vector held planners for both hierarchically invariant 
as well as transitional navigation in the configuration space. 
We apply these methods to the centralized coordination and 
control of n perfectly sensed and actuated Euclidean spheres 
in a d-dimensional ambient space (for arbitrary n and d). 
Given a desired configuration supporting a desired hierarchy, 
we construct a hybrid controlier which is quadratic in n and 
aigebraic in d and prove that its execution brings ali but a 
measure zero set of initial configurations to the desired goal with 
the guarantee of no collisions along the way. 

Index Terms —multi-agent systems, navigation functions, for¬ 
mation control, swarm robots, configuration space, coordinated 
motion planning, hierarchical clustering, cohesion, segregation. 


I. Introduction 

C ooperative, coordinated action and sensing can promote 
efficiency, robustness, and flexibility in achieving com¬ 
plex tasks such as search and rescue, area exploration, surveil¬ 
lance and reconnaissance, and warehouse management [2], 
Despite significant progress in the analysis of how local rules 
can yield such global spatiotemporal patterns [3]-[5], there 
has been strikingly less work on their specification. With few 
exceptions, the engineering literature on multirobot systems 
relies on task representations expressed in terms of rigidly 
imposed configurations — either by absolutely targeted posi¬ 
tions, or relative distances — missing the intuitively substantial 
benefit of ignoring fine details of individual positioning, to 
focus control effort instead on the presumably far coarser 
properties of the collective pattern that matter. We seek a 
more relaxed means of specification that is sensitive to spatial 
distribution at multiple scales (as influencing the intensity of 
interactions among individuals and with their environment [6]) 
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A preliminary version of this paper is presented in the conference paper 
[1] for point particles and a certain choice of hierarchical clustering. In this 
paper, we propose a general hierarchical navigation framework for a broad 
class of clustering methods and disk-shaped robots. 
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Fig. 1. Moving from one spatial distribution to another is generally carried 
through rearrangements of robot groups (clusters) at different resolution 
corresponding to transitions between different cluster structures (hierarchies). 


and the identities of neighbors (as determining the capabilities 
of heterogeneous teams [7]) while affording, nevertheless, a 
well-formed deterministic characterization of pattern. 

We are led to the notion of hierarchical clustering. We rein¬ 
terpret this classical method for unsupervised learning [8] as a 
formalism for the specification and reactive implementation of 
collective mobility tasks expressed with respect to successively 
refined partitions of the agent set in a manner depicted in 
Fig. 1. There, we display three different configurations of 
five planar disks whose relative positions are specified by 
three distinct trees that represent differently nested clusters 
of relative proximity. The first configuration exhibits three 
distinct clusters at a resolution in the neighborhood of 2 
units of distance: the red and the blue disks; the yellow and 
the orange disks; and the solitary green disk. At a coarser 
resolution, in the neighborhood of 4 units of distance, the 
green disk has merged into the subgroup including the red 
and the blue disks to comprise one of only two clusters 
discernible at this scale, the other formed by the orange and 
the yellow disks. It is intuitively clear that this hierarchical 
arrangement of subgroupings will persist under significant 
variations in the position of each individual disk. It is similarly 
clear that the second and third configurations (and significant 
variations in the positions of the individual disks of both) 
support the very differently nested clusters represented by the 
second and third trees, respectively. In this paper, we introduce 
a provably correct and computationally effective machinery 
for specifying, controlling invariantly to, and passing between 
such hierarchical clusterings at will. 

As an illustration of its utility, we use this formalism to 
solve a specific instance of the reactive motion planning 
problem suggesting how the new “relaxed” hierarchy-sensitive 
layer of control can be merged with a task entailing a tradi¬ 
tional rigidly specified goal pattern. Namely, for a collection 






























































































ESE TECHNICAL REPORT — JULY 8, 2015 


2 


TABLE I 

Constituent Problems of Hierarchical Robot Navigation 


Problem 

Solution 

Theorem 

Description 

i 

Table IV 

4 

Hierarchy invariant vector field planner 

2 

Table V 

5 

Reactive navigation across hierarchies 

3 

Eqn.(33) 

6 

Cross-hierarchy geometric realization 


of n disk robots in we presume that a target hierarchy has 
been specified along with a goal configuration that supports it, 
and that the robot group is controlled by a centralized source of 
perfect, instantaneous information about each agent’s position 
that can command exact instantaneous velocities for each 
disk. We present an algorithm resulting in a purely reactive 
hybrid dynamical system [9] guaranteed to bring the disk 
robots to both the hierarchical pattern as well as the rigidly 
specified instance from (almost) arbitrary initial conditions 
with no collisions of the disks along the way. Stated formally 
in Table III, the correctness of this algorithm is guaranteed by 
Theorem 1 whose proof appeals to the resolution of various 
constituent problems summarized in Table I. The construction 
is computationally effective: the number of discrete transitions 
grows in the worst case with the square of the number of 
robots, n; each successive discrete transition can be computed 
reactively (i.e., as a function of the present configuration) 
in time that grows linearly with the number of robots; and 
the formulae that define each successive vector field and 
guard condition are rational functions (defined by quotients 
of polynomials over the ambient space of degree less than 3) 
entailing terms whose number grows quadratically with the 
number of robots. 

This paper is organized as follows. We review in the next 
section the relevant background literature: first on reactive 
multirobot motion planning to relate the difficulty and impor¬ 
tance of our sample problem to the state of the art in this field; 
next on the role of hierarchy in configuration spaces as ex¬ 
plored both in biology and engineering. Because the notion of 
hierarchical clustering is a new abstraction for motion planning 
we devote Section III to a presentation of the key background 
technical ideas: first we review the relevant topological prop¬ 
erties of configuration spaces; next the relevant topological 
properties of tree spaces; and, finally, prior work establishing 
properties of certain functions and relations between them. 
Because we feel that the specific motion planning problem we 
pose and solve represents a mere illustration of the larger value 
of this abstraction for multirobot systems we devote Section 
IV to a presentation of some of the more generic tools from 
which our particular construction is built: first we introduce 
the notion of hierarchy invariant navigation; next we discuss 
the combinatorial problem of hierarchy rearrangement as a 
graph navigation problem; and finally we interpret a subgraph 
of that combinatorial space as a “prepares” graph [10] for the 
hierarchy-invariant cover of configuration space. In Section V 
we pose and solve the specific motion planning problem using 
the concepts introduced in Section III and the tools introduced 
in Section IV. Section VI offers some numerical studies of 
the resulting algorithm. We conclude in Section VII with a 
summary of the major technical results that yield the specific 
contribution followed by some speculative remarks bearing on 


the likelihood that recent extensions of these ideas presently 
in progress [11] might afford a distributed reformulation, thus 
addressing the first (and better explored) remarkable biological 
inspiration for multirobot systems. 

II. Related Literature 
A. Multirobot Motion Planning 

1) Complexity: The intrinsic complexity of multibody con¬ 
figurations impedes computationally effective generalizations 
of single-robot motion planners [ 1 2], [13]. Coordinated motion 
planning of thick bodies in a compact space is computationally 
hard. For example, moving planar rectangular objects within a 
rectangular box is PSPACE-hard [ 1 4] and motion planning for 
finite planar disks in a polygonal environment is strongly NP- 
hard [15]. Even determining when and how the configuration 
space of noncolliding spheres in a unit box is connected entails 
an encounter with the ancient sphere packing problem [16]. 
Within the domain of reactive or vector field motion planning, 
it has proven deceptively hard to determine exactly this line 
of intractability. Consequently, this intrinsic complexity for 
coordinated vector field planners is generally mitigated by 
either assuming objects move in an unbounded (or sufficiently 
large) space [17], [18], as we do in Section V, or sim¬ 
ply assuming conditions sufficient to guarantee connectivity 
between initial and goal configurations [19], [20], On the 
other hand, more relaxed versions entailing (perhaps partially) 
homogeneous (unlabeled) specifications for interchangeable 
individuals have yielded computationally efficient planners 
in the recent literature [21]-[24], and we suspect that the 
cluster hierarchy abstraction may be usefully applicable to 
such partially labeled settings. 

2) Reactive Multirobot Motion Planning: Since the prob¬ 
lem of reactively navigating groups of disks was first intro¬ 
duced to robotics [25], [26], most research into vector field 
planners has embraced the navigation function paradigm [27], 
A recent review of this two decade old literature is provided 
in [17], where a combination of intuitive and analytical results 
yields a nonsmooth centralized planner for achieving goal 
configurations specified up to rigid transformation. As noted in 
[17], the multirobot generalization of a single-agent navigation 
function is challenged by the violation of certain assumptions 
inherited from the original formulation [27]. One such assump¬ 
tion is that obstacles are “isolated” ( i.e. nonintersecting). In 
the multirobot case, every robot encounters others as mobile 
obstacles, and any collision between more than two robots 
breaks down the isolated obstacle assumption [17], In some 
approaches, the departure from isolated interaction has been 
addressed by encoding all possible collision scenarios, yielding 
controllers with terms growing super-exponentially in the 
number of robots, even when the workspace is not compact 
[ 18], In contrast, our recourse to the hierarchical representation 
of configurations affords a computational burden growing 
merely quadratically in the number of agents. In [19], the 
problem is circumvented by allowing critical points on the 
boundary (with no damage to the obstacle avoidance and 
convergence guarantees), but, as mentioned above, very con¬ 
servative assumptions about the degree of separation between 
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agents at the goal state are required. In contrast, our recourse to 
hierarchy allows us to handle arbitrary (non-intersecting) goal 
configurations, albeit our reliance upon the homotopy type of 
the underlying space presently precludes the consideration of 
a compact configuration space as formally allowed in [19]. 1 

Another limitation of navigation function approaches is the 
requirement of proper parameter tuning to eliminate local 
minima. Some effort has been given to automatic adaptation 
of this parameter [20], and, in principle, the original results 
of [27] guarantee that any monotone increasing scheme must 
eventually resolve the issue of local minima, however, this 
is numerically unfavorable (the Hessian of the resulting field 
becomes stiffer) and incurs substantial performance costs 
(transients must slow as the tuning parameter increases). 2 In 
contrast, our recourse to hierarchy removes the need for any 
comparable tuning parameter. 

Many of the concepts and some of the technical construc¬ 
tions we develop here were presented in preliminary form in 
the conference paper [1], building on the initial results of 
the conference paper [29]. This presentation gives a unified 
view of the detailed results (with some tutorial background) 
and contributes a major new extension by generalizing the 
construction of [ 1 ] from point particles to thickened disks of 
non-zero radius (necessitating a more involved version of the 
hierarchy invariant fields in Section V-B). 

B. The Use of Hierarchies as Organizational Models 

1) Hierarchy in Configuration Space: That a hierarchy of 
proximities might play a key role in computationally efficient 
coordinated motion planning had already been hinted at in 
early work on this problem [30]-[32], Partial hierarchies 
that limit the combinatorial growth of complexity have been 
explicitly applied algorithmically to organize and simplify the 
systematic enumeration of cluster adjacencies in the configu¬ 
ration space [33]. Moreover, hierarchical discrete abstraction 
methods are successfully applied for scalable steering of a 
large number of robots as a group all together by controlling 
the group shape [34], and also find applications for congestion 
avoidance in swarm navigation [35], While the utility of 
hierarchies and expressions for manipulating them are by no 
means new to this problem domain, we believe that the explicit 
formal connection [36] we exploit between the topology of 
configuration space [37] and the topology of tree space [38] 
through the hierarchical clustering relation [8] is entirely new. 

2) Hierarchy in Biology and Engineering: Biology offers 
spectacularly diverse examples of animal spatial organization 
ranging from self-sorting in cells [39], tissues and organs 
[40], [41], and groups of individuals [42]-[44] to more pat¬ 
terned teams [7], [45]-[47], all the way through strategic 
group formations in vertebrates [48], [49], mammals [50]- 
[53], and primates [54], [55] hypothesized to increase efficacy 

1 We conjecture that a compact configuration space with a free-space goal 
point satisfying the conditions of [19] has the same homotopy type as the 
unbounded case we treat here. 

-It bears mention in passing that partial differential equations (e.g., har¬ 
monic potentials [28]) yield self-tuning navigation functions but these are 
intrinsically numerical constructions that forfeit the reactive nature of the 
closed form vector field planners under discussion here. 


in foraging [45], [46], hunting [48], [50], [51], [54], logistics 
and construction [7], [47], predator avoidance [56], [57], and 
even to stabilize whole ecologies [58] — all consequent upon 
the collective ability to target, track, and transform geomet¬ 
rically structured patterns of mutual location in response to 
environmental stimulus. These formations are remarkable for 
at least two reasons. First, their global structure seems to 
arise from local signaling and response amongst proximal 
individuals coupled to specific physical environments [59], in 
a manner that might be posited as a paradigm for generalized 
emergent intelligence [60], Second, these formations appear 
to resist familiar rigid prescriptions governing absolute or 
relative location, instead giving wide latitude for individual 
autonomy and detailed positioning (intuitively, a necessity for 
negotiating fraught, highly dynamic interactions such as arise 
in, say, hunting [50], [52]), while, nevertheless, supporting 
the underlying coarse, deterministic “deep structure” as a 
dynamical invariant. It is this second remarkable attribute of 
biological swarms that inspires the present paper. 

This profusion of pattern formation in biology has inspired 
a commensurate interest in robotics, yielding a growing lit¬ 
erature on group coordination behaviors [6 1 ]— [64] motivated 
by the intuition that the heterogeneous action and sensing 
abilities of a group of robots might enable a comparably 
diverse range of complex tasks beyond the capabilities of a 
single individual. For example, group coordination via splitting 
and merging behaviours creates effective strategies for obstacle 
avoidance [65], congestion control [35], shepherding [66], area 
exploration [66], [67], and maintaining persistent and coherent 
groups while adapting to the environment [64]. In almost all of 
the robotics work in this area, formation tasks are given based 
upon rigid specifications taking either the form of explicit 
formation or relative distance graphs, with few exceptions 
including the “shape” abstraction of [34] or applications in 
unknown environments such as area coverage and exploration 
[68]. Alternatively, hierarchical clustering offers an interesting 
means of ensemble task encoding and control; and it seems 
likely that the ability to specify organizational structure in the 
precise but flexible terms that hierarchy permits will add a 
useful tool to the robot motion planner’s toolkit. 


TABLE II 

Principal symbols used throughout this paper 


J, r 

Conf (R d , J, r) 

HC 

HtU-means 

6(r) 

Portal( ct, r) 
Port CT t 

Aj =’CBT j,E a ) 
= CBTj.g^) 


Sets of labels and disk radii [III-A] 

The conf. space of labelled, noncolliding disks (1) 
The space of binary trees [III-B] 

Hierarchical clustering [III-C] 

Iterative 2-means clustering [V] 

The stratum of a tree, r E B7j, (2) 

Portal configurations of a pair, (<7, r), of trees (5) 
Portal map [IV-A3] 

The adjacency graph of trees [III-D] 

The NNI-graph of trees [III-D] 


III. Hierarchical Abstraction 

This section describes how we relate multirobot config¬ 
urations to abstract cluster trees via hierarchical clustering 
methods and how we define connectivity in tree space. 
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Fig. 2. An illustration of (a) a heteregeneous configuration of unit disks 
in Conf (R 2 , [6] , l) and (b) its iterative 2-mean clustering [69] hierar¬ 
chy t in 237[6], where the dashed lines in (a) depict the separating 
hyperplanes between clusters, and (b) illustrates hierarchical cluster re¬ 
lations: parent - Pr(/,r), children - Ch(/,r), and local complement 
(sibling) - I~ T of cluster I of the rooted binary tree, r E An 

interior node is referred by its cluster, the list of leaves below it; for 
example, I = {3,5}. Accordingly the cluster set of r is C (r) = 
{{1}, {2},..., {6}, {1,6}, {3, 5}, {2, 4}, {1, 3, 5, 6}, {1, 2, 3, 4, 5, 6}}. 
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Fig. 3. The Quotient Space Conf(C, [3] ,0)/ where for any x,y £ 
Conf (C, [3] , 0), x ~ y 4=> = ^-yi ■ Here, point particle 

configurations are quotiented out by translation, scale and rotation, and so 
xi = 0 + Oi. X 2 = 1 + Oi and X3 £ C \ {xi,X2}. Regions are colored 
according the associated cluster hierarchies results from their iterative 2-mean 
clustering [69], For instance, any configuration in the white region supports 
all hierarchies in ®Tr 3 i. 


A. Configuration Space 

For ease of exposing fundamental technical concepts, we 
restrict our attention to groups of Euclidean spheres in a 
d-dimensional ambient space, but many concepts introduced 
herein can be generalized to any metric space. 

Given an index set, J = [n]: = {1,..., n} C N, a heteroge¬ 
neous multirobot configuration, x = (xj) jgJ , is a labeled non¬ 
intersecting placement of | J| = n distinct Euclidean spheres, 3 
where ith sphere is centered at x, £ R f/ and has radius 
Vi > 0. We find it convenient to identify the configuration 
space [ 37 ] with the set of distinct labelings, i.e., the injective 
mappings of J into W 1 , and, given a vector of nonnegative 
radii, r := ( r j)j eJ € (R>o) J i we will Hud it convenient to 
denote our “thickened” subset of this configuration space as 4 


Conf ( 


\ J, r): =|xe (R d f ||x;-Xj || > n+rj jj,(l) 


where ||.|| denotes the standard Euclidean norm on 


B. Cluster Hierarchies 

A rooted semi-labelled tree r over a fixed finite index set J, 
illustrated in Fig. 2, is a directed acyclic graph G T = ( V T . E T ), 
whose leaves, vertices of degree one, are bijectively labeled 
by J and interior vertices all have out-degree at least two; 
and all of whose edges in E T are directed away from a vertex 
designated to be the root [70]. A rooted tree with all interior 
vertices of out-degree two is said to be binary or, equivalently, 
non-degenerate, and all other trees are said to be degenerate. 
In this paper 53 T j denotes the set of rooted nondegenerate 
trees over leaf set J. 

A rooted semi-labelled tree r uniquely determines (and 
henceforth will be interchangeably termed) a cluster hierarchy 
[71], By definition, all vertices of r can be reached from the 
root through a directed path in r. The cluster of a vertex 
v £ V T is defined to be the set of leaves reachable from v 

3 Here. .4 denotes the cardinality of set A. 

4 FIere, R and R>o denote the set of real numbers and its subset of non¬ 

negative real numbers, respectively; and is the d-dimensional Euclidean 

space. 


by a directed path in r. Let C (r) denote the set of all vertex 
clusters of r. 

For every cluster I £ C (r) we recall the standard notion of 
parent (cluster) Pr(/,r) and lists of children Ch(/,r), an¬ 
cestors Anc (/, t) and descendants Des (I, r) of I in r — see 
[29] for explicit definitions of cluster relations. Additionally, 
we find it useful to define the local complement (sibling) of 
cluster I £ C (r) as I~ T : = Pr (I, r) \ I. 


C. Configuration Hierarchies 

A hierarchical clustering 5 HC C Conf (R d , J, r) x 53Tj 
is a relation from the configuration space Conf (R d , J, r) to 
the abstract space of binary hierarchies 13T/ [8], an example 
depicted in Fig. 2. In this paper we will only be interested in 
clustering methods that can classify all possible configurations 
(i.e. for which HC assigns some tree to every configuration), 
and so we need: 


Property 1 HC is a multi-function. 


Most standard divisive and agglomerative hierarchical clus¬ 
tering methods exhibit this property, but generally fail to be 
functions because choices may be required between different 
but equally valid cluster splitting or merging decisions [8], 
Given such an HC, for any x £ Conf (K. d , J, r) and r £ 
23Tj, we say x supports r if and only if (x, r) £ HC. The 
stratum associated with a binary hierarchy r £ 1)‘X /, denoted 
by ©(t) C Conf (S. d , J, r), is the set of all configurations 
x £ Conf (R d , J, r) supporting the same tree r [29], 


©(r) := | 


x £ Conf( 


■I r ) 


(x,t) £ HC| , 


( 2 ) 


and this yields a tree-indexed cover of the configuration space. 
For purposes of illustration, we depict in Fig. 3 the strata of 
Conf(C, [3] , 0 ) — a space that represents a group of three 
point particles on the complex plane. 6 


5 Although clustering algorithms generating degenerate hierarchies are avail¬ 
able, many standard hierarchical clustering methods return binary clustering 
trees as a default, thereby avoiding commitment to some “optimal” number 
of clusters [8], [72]. 

6 Here, 0 and 1 are, respectively, vectors of all zeros and ones with the 
appropriate sizes. 
















































ESE TECHNICAL REPORT — JULY 8, 2015 


5 



Fig. 4. An illustration of NNI moves between binary trees: each arrow is 
labeled by a source tree and an associated cluster defining the move. 


The restriction to binary trees precludes combinatorial tree 
degeneracy [70] and we will avoid configuration degeneracy 
by imposing: 

Property 2 Each stratum of HC includes an open subset of 
configurations, i.e. for every t £ 23T j, G(r) 7 ^ 0 . 7 

Once again, most standard hierarchical clustering methods 
respect this assumption: they generally all agree (i.e. return 
the same result) and are robust to small perturbations of a 
configuration whenever all its clusters are compact and well 
separated [72], 

Given any two multirobot configurations supporting the 
same cluster hierarchy, moving between them while maintain¬ 
ing the shared cluster hierarchy (introduced later as Problem 

1 ) requires: 

Property 3 Each stratum of HC is connected. 

For an arbitrary clustering method this requirement is gen¬ 
erally not trivial to show, but when configuration clusters of 
HC are linearly separable, one can characterize the topological 
shape of each stratum to verify this requirement, as we do in 
Section V-A. 

D. Graphs On Trees 

After establishing the relation between multirobot configu¬ 
rations and cluster hierarchies, the final step of our proposed 
abstraction is to determine the connectivity of tree space. 

Define the adjacency graph Aj = (23Tj, Ejfj to be the 1- 
skeleton of the nerve [73] of the Conf (R d , J, r)-cover induced 
by HC. That is to say, a pair of hierarchies, er, r £ "BTj, 
is connected with an edge in £ji if and only if their strata 
intersect, 6(a) (T G(r) 7 ^ 0. To enable navigation between 
structurally different multirobot configurations later (Problem 

2 ) , we need: 

Property 4 The adjacency graph is connected. 

Although the adjacency graph is a critical building block of 
our abstraction, as Fig. 3 anticipates, HC strata generally have 
complicated shapes, making it usually hard to compute the 
complete adjacency graph. 

Fortunately, the computational biology literature [38] offers 
an alternative notion of adjacency that turns out to be both 
feasible and nicely compatible with our needs, yielding a com¬ 
putationally effective, connected subgraph of the adjacency 
graph, Aj, as follows. 

The Nearest Neighbor Interchange (NNI) move at a cluster 
A £ C (a) on a binary hierarchy a £ 23T j, as illustrated in Fig. 

7 Here, A denotes the interior of set A. 



Fig. 5. The NNI Graph: a graphical representation of the space of rooted 
binary trees, TSTj, with NNI connectivity, where J = [4] = {1, 2, 3, 4}. 


4, swaps cluster A with its parent’s sibling C = Pr (A,a)~ a 
to yield another binary hierarchy r £ 23 7j [74], [75], Say 
that a,r £ HTj are NNI-adjacent if and only if one can be 
obtained from the other by a single NNI move. Moreover, 
define the NNI-graph Nj = (®Tj, £jj-) to have vertex set 
23 T j, with two trees connected by an edge in £>r if and only 
if they are NNI-adjacent, see Fig. 5. An important contribution 
of this paper will be to show how the NNI-graph yields a 
computationally effective subgraph of the adjacency graph 
(Theorem 6). 

IV. Hierarchical Navigation Framework 

Hierarchical abstraction introduced in Section III intrinsi¬ 
cally suggests a two-level navigation strategy for coordinated 
motion design: (i) at the low-level perform finer adjustments 
on configurations using hierarchy preserving vector fields, 
(ii) and at the high-level resolve structural conflicts between 
configurations using a discrete transition policy in tree space; 
and the connection between these two levels are established 
through “portals” — open sets of configurations supporting 
two adjacent hierarchies. In this section we abstractly describe 
the generic components of our navigation framework and we 
show how they are put together. 

A. Generic Components of Hierarchical Navigation 

1) Hierarchy Preserving Navigation: For ease of exposition 
we restrict attention to first order (completely actuated single 
integrator) robot dynamics, and we will be interested in smooth 
closed loop feedback laws (or hybrid controllers composed 
from them) that result in complete flows, 8 

x = / (x), (3) 

where / : Conf (K d , J, r) — > (R d ) J is a vector field over 
Conf(M d , J, r) (1). 

Denote by ip* th e. flow [79] on Conf(K d , J, r) induced by 
the vector field /. For a choice of hierarchical clustering HC, 
the class of hierarchy-invariant vector fields maintaining the 

8 A long prior robotics literature motivates the utility of this fully actuated 
“generalized damper” dynamical model [76], and provides methods for “lifts” 
to controllers for second order plants [77], [78] as well. 
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robot group in a specified hierarchical arrangement of clusters, 
r G T>7j, is defined as [29], 

Thc(t) : = | /: Conf (V, J, (V)' ' | gf (6(rjc6(r), f > oj.(4) 

Hierarchy preserving navigation, the low-level component of 
our framework, uses the vector fields of Thc (r) to invariantly 
retract almost all of a stratum onto any designated goal 
configuration.'’ Thus, we require the availability of such a 
construction, summarized as: 

Problem 1 For any r £ 23 T j and y £ 6(t) associated with 
HC construct a control policy, f T ,y, using the hierarchy invari¬ 
ant vector fields of 3 hc (t) whose closed loop asymptotically 
results in a retraction, R T . y , of & (r), possibly excluding a set 
of measure zero 10 , onto {y}. 

Key for purposes of the present application is the observation 
that any hierarchy-invariant field / £ IX H c (t) must leave 
Conf (K d , J, r) invariant as well, and thus avoids any self¬ 
collisions of the agents along the way. 

2) Navigation in the Space of Binary Trees: Whereas the 
controlled deformation retraction, R T , y , above generates paths 
“through” the strata, we will also want to navigate “across” 
them along the adjacency graph (which will be later in Section 
V replaced with the NNI-graph — a computationally efficient, 
connected subgraph). Thus, we further require a construction 
of a discrete feedback policy in 23T, / that recursively generates 
paths in the adjacency graph toward any specified destination 
tree from all other trees in '13‘X./ by reducing a “discrete 
Lyapunov function” relative to that destination, which we 
summarize as follows: 

Problem 2 Given any r £ '13Tj construct recursively a 
closed loop discrete dynamical system in the adjacency graph, 
taking the form of a deterministic discrete transition rule, g T , 
with global attractor at t endowed with a discrete Lyapunov 
function relative to the attractor r. 

Such a recursively generated choice of next hierarchy will play 
the role of a discrete feedback policy used to define the reset 
map of our hybrid dynamical system. 

3) Hierarchical Portals: Here, we relate the (combinatorial) 
topology of hierarchical clusters to the (continuous) topology 
of configurations by defining “portals” — open sets of con¬ 
figurations supporting two adjacent hierarchies. 

Definition 1 The portal. Portal (a, t), of a pair of hierar¬ 
chies, <j,t £ 13 7j, is the set of all configurations supporting 
interior strata of both trees. 

Portal (<t, t) : = 6(a) fl©(r) . (5) 

It is important to remark that, instead of a single goal configuration, a more 
general family of problems can be parametrized by a set of goal configurations 
sharing a certain homotopy model comprising a set of appropriately nested 
spheres; and for such a general case one can still construct an exact retraction 
within our framework. 

'’Recall from [ ] that a continuous motion planner in a configuration 

space X exists if and only if X is contractible. Hence, if a hierarchical 
stratum is non-contractible (Theorem 2), the domain of such a vector field 
planner described in Problem 1 must exclude at least a set of measure zero. 


Namely, portals are geometric realizations in the configuration 
space of the edges of the adjacency graph on trees, see Fig. 
3. To realize discrete transitions in tree space via hierarchy 
preserving navigation in the configuration space, we need 
a portal map that takes an edge of the adjacency graph, 
and returns a target configuration in the associated portal, 
summarized as: 

Problem 3 Given an edge (a, t) £ Ea of the adjacency graph 
Aj = ('B'Jj, Ea), construct a geometric realization map 
Port^T-) : ©(er) —> Portal (a, r) that takes a configuration 
supporting a, and returns a target configuration supporting 
both trees a and r. 

A portal map will serve the role of a dynamically computed 
“prepares graph” [10] for the sequentially composed local 
controllers whose correct recruitment solves the reactive co¬ 
ordinated motion planning problem (Theorem 1). 

B. Specification and Correctness of the Hierarchical Naviga¬ 
tion Control (HNC) Algorithm 

Assume the selection of a goal configuration y £ &(t) and 
a hierarchy t £ 23T j that y supports. Now, given (almost) any 
initial configuration x £ 6(a) for some hierarchy a £ 23 T j 
that x supports. Table III presents the HNC algorithm. 

TABLE III 

The HNC Algorithm 

For (almost) any initial x G S(oj and a G (BTj, and desired y G S(r) 
and t G ®Xj, 

1) (Hybrid Base Case) if x G S(r) then apply stratum-invariant 
dynamics, f T , y (Problem 1). 

2) (Hybrid Recursive Step) else, 

a) invoke the discrete transition rule g T (Problem 2) to propose 
an adjacent tree, 7 G 3BXj, with lowered discrete Lyapunov 
value. 

b) Choose local configuration goal, z := Port( CTj7 ) (x) (Prob¬ 
lem 3). 

c) Apply the stratum-invariant continuous controller / CTjZ (Prob¬ 
lem 1 ). 

d) If the trajectory enters S (r) then go to step 1; else, the 
trajectory must enter 6 ( 7 ) in finite time in which case 
terminate / CT ,z, reassign <7 <— 7 , and go to step 2 a). 



Fig. 6. Flowchart of the hybrid vector field planner. 
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Theorem 1 The HNC Algorithm in Table III defines a hybrid 
dynamical system whose execution brings almost every initial 
configuration, x G Conf (W. d , j), infinite time to an arbitrarily 
small neighborhood of y G ©(r) with the guarantee of no 
collisions along the way. 

Proof In the base case, 1 ) the conclusion follows directly 
from the construction of Problem 1: the flow f T y keeps the 
state in ©(r), approaches a neighborhood of y (which is an 
asymptotically stable equilibrium state for that flow) in finite 
time. 

In the inductive step, a) The NNI transition rule g T guaran¬ 
tees a decrement in the Lyapunov function after a transition 
from a to 7 (Problem 2), and a new local policy f az is 
automatically deployed with a local goal configuration z G 
Portal (<t, 7) found in b). Next, the flow f c r , z in c) is guaran¬ 
teed to keep the state in ©(er) and approach z G Portal (<r, 7) 
asymptotically from almost all initial configurations. If the 
base case is not triggered in d), then the state enters arbitrarily 
small neighborhoods of z and, hence, must eventually reach 
Portal (<7,7) C ©(7) in finite time, triggering a return to 
2a). Because the dynamical transitions g T initiated from any 
hierarchy in 13Tj reaches r in finite steps (Problem 2), it must 
eventually trigger the base case. ■ 

V. Hierarchical Navigation of Euclidean Spheres 
via Bisecting K-means Clustering 

We now confine our attention to 2-means divisive hierarchi¬ 
cal clustering [69], HC 2 -means, and demonstrate a construction 
of our hierarchical navigation framework for coordinated nav¬ 
igation of Euclidean spheres via HC 2 - me ans- 

A. Hierarchical Strata of HC 2 - me ans 

Iterative 2-means clustering, HC 2 - m eans- is a divisive method 
that recursively constructs a cluster hierarchy of a configu¬ 
ration in a top-down fashion [69], Briefly, this method splits 
each successive (partial) configuration by applying 2 -means 
clustering, and successively continues with each subsplit until 
reaching singletons. By construction, complementary con¬ 
figuration clusters of HC 2 . me ans are linearly separable by a 
hyperplane defined by the associated cluster centroids 11 , as 
illustrated in Fig. 2; and the stratum of HC 2 -means associated 
with a binary hierarchy r G 23 T j can be characterized by the 
intersection inverse images, 

©( r ) = n nc/.-rt 0 ’ 00 )’ (6) 

/GC(r)\{ J} i£l 

of the scalar valued “separation” function, 77 i,/, r : 

Conf (M d , J, r) —> R [29] returning the distance of agent i 
in cluster / G 6 (r) \ { J} to the perpendicular bisector of the 
centroids of complementary clusters I and I~ T : 12 

r \ t t \\ T ( x ) tn\ 

Vi,J, T (x):=(x i —m /iT (x)) (7) 

11 In the context of self-sorting in heterogeneous swarms [61], two groups 
of robot swarms are said to be segregated if their configurations are linearly 
separable; and in this regal'd configuration hierarchies of HC 2 - m eans represent 
spatially cohesive and segregated swarms groups at different resolutions. 

12 Here, A T denotes the transpose of A. 


where the associated “cluster functions” of a partial configu¬ 
ration, x |I = (xi) ieI , are defined as 


c(x| I) 

s I,T (x) 

m I,r (x) 


1 

W\ 


Z x ” 

iei 


c(x|/) -c(x|/ T ) , 
c (x|J) + c (x| I~ T ) 


2 


( 8 ) 

( 9 ) 

00 ) 


We now follow [36] in defining terminology and express- 
sions leading to the characterization of the homotopy type of 
the stratum, ©(r), associated with a nondegenerate hierarchy. 
The proofs of our formal statements all follow the same pattern 
as established in [36], and we omit them to save space here. 

Definition 2 A configuration x G Conf (\R rl , J, r) is narrow 
relative to the split, {I, J \ /}, if 

max: r(x|A) < ]■ ||c (x|J) - c (x| J \ J)||, (11) 

where the radius of a cluster, A C J, is defined to be 13 

r (x|A) := max (||x a - c(x|A)|| + r a ) . ( 12 ) 

a£A 

Say that x G ©(r) is a standard configuration relative to the 
nondegenerate hierarchy, r G 23Tj, if it is narrow relative to 
each local split, Ch (/, r), of every cluster, J G C (t). 


Proposition 1 If x G 6(r) is a standard configuration then 
for each cluster, I G G (t), any rigid rotation of the partial 
configuration, x|J, around its centroid, c(x| 7 ), as illustrated 
in Fig. 7, preserves the supported hierarchy r. 

Proposition 2 For any finite label set J C N and non- 
degenerate tree r G 23 7 j, there exists a strong deformation 
retraction 14 

R r : ©(t) x [0,1] -A- S(t) (13) 

of ©(t) onto the subset of standard configurations of © (r). 

These two observations now yield the key insight reported 
in [36]. 


'Recall from Section III-A that v, denotes the radius of 7 th sphere for any 

i S J. 

14 In [36] authors study point particle configurations, and they construct 
a strong deformation retraction onto standard configurations by shrinking 
clusters around cluster centroids; and one can obtain similar result for 
thickened spheres by properly expanding cluster configurations instead of 
shrinking. 



Fig. 7. An illustration of (left) narrow and (right) standard disk configurations, 
where arrows and dashed circles indicate clusters that can be rigidly rotated 
around their centroids while preserving their clustering structures. 
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Theorem 2 The set of configurations x G Conf (M. d , J, r) 
supporting a non-degenerate tree has the homotopy type of 

(§ d_1 )l J l~ 1 . 

To gain an intuitive appreciation, one can restate this 
result as follows: two configurations in ©(t) are topologi¬ 
cally equivalent if and only if the corresponding separating 
hyperplane normals of configuration clusters are the same. 15 
Hence, navigation in a hierarchical stratum is carried out 
by aligning separating hyperplane normals, illustrated in Fig. 
8; and using this geometric intuition, we construct in [29] 
a family of hierarchy preserving control policies for point 
particle configurations, and in the following we extend that 
construction to thickened disk configurations. 




Fig. 8 . The topological shape of a hierarchical stratum intuitively suggests 
that global navigation in a hierarchical stratum is accomplished by aligning 
separating hyperplanes of configurations. 


Theorem 3 Iterative 2-means clustering HC 2 - me ans is a multi¬ 
function, and each of its stratum, ©(r) associated with r G 
23‘T j, is connected and has an open interior. 

Proof It is well known that fc-means clustering is a multi¬ 
function generally yielding different fc-partitions of any given 
data, and so is HC 2 - me ans (Property 1) [8], [72], Further, it 
follows from Definition 2 and Proposition 2 that standard 
configurations in ©(r) is open (Property 2), and Theorem 2 
guarantees the connectedness of ©(r) (Property 3). ■ 


B. Hierarchy Preserving Navigation 

We now introduce a recursively defined vector field for 
navigation in a hierarchical stratum and list its invariance and 
stability properties. 

Suppose that some desired configuration, y G ©(r) has 
been selected, supporting some desired nondegenerate tree, 
r G 23‘Xj. Our dynamical planner takes the form of a 
centralized hybrid controller, f Ty : ©(r) —> defining 

a hierarchy-invariant vector held whose flow in ©(r) yields 
the desired goal configuration, y, recursively defined according 
to logic presented in Table IV. Throughout this section, the tree 
r and the goal configuration y are fixed, and we therefore sup¬ 
press all mention of these terms wherever convenient, in order 
to compress the notation. For example, for any x G ©(t), 
/ G C (t) and i G I we use the shorthand r]ij (x) = rjij, T (x) 
(7), si (x) = s/, T (x) (9), m/ (x) = m j iT (x) (10) and so on. 

l5 Note that a binary hierarchy over the leaf set J has | J\ — 1 interior nodes, 
i.e. nonsingleton clusters [74]. 


TABLE IV 

The Hierarchy-Preserving Navigation Vector Field 

For any initial x E ©(r) and desired y E ©(r), supporting r E 33Tj, 
the hierarchy preserving vector field, f r ,y : ©(t) —> , 

fr,y ( x ) : = fr,y (x, 0, J ) , 


is recursively computed using the post-order traversal 16 of r starting 
at the root cluster J with the zero control input 0 E (R d ) as follows: 
for any u e (R 1 *) J and / e C (r), 



i) 

function u = / T>y (x, u, I) 



$ 

r 2 ) 

if x E V A (I) (15), 



3 

3) 

in- Ia (x, u, I) (14), 

% 

Attracting Field 

s 

4) 

else if x ^ D H (I) (18), 



CQ 

i 5) 

in- f s (x, u,7) (24), 

% 

Split Separation Field 


6) 

else 



c 

o 

7) 

{II, Ir} <— Ch (/, r). 



1 < 

8) 

u L J fr, y (x.u ,I L ), 

% 

Recursion for Left Child 

o 

o 

9) 

Ufl J fr, y (x, li L ,I R ), 

% 

Recursion for Right Child 

DC 

10) 

U <- Ih (x.u r ,I) (19), 

% 

Split Preserving Field 


III) 

end 




12 ) 

return u 




In brief, the hierarchy invariant vector held f Ty recursively 
detects partial configurations whose separating hyperplanes 
are “sufficiently aligned” with the desired ones, as specified 
in (15) and illustrated in Fig. 9, and that can be directly 
moved towards the desired configurations, using a family 
of attracting fields /a (14), with no collisions along the 
way. Once the partial configurations associated with sibling 
clusters I and I~ T of r are in the domains of their associated 
attracting fields, / T . y rotates these partial configurations while 
preserving the hierarchy so that their separating hyperplane is 
also asymptotically aligned. Hence, f T y asymptotically aligns 
the separating hyperplanes of clusters of r in a bottom-up 
fashion; and once the separating hyperplanes of all clusters of 
r are “sufficiently aligned”, / T . y drives asymptotically each 
disk directly towards its desired location. We now present and 
motivate its constituent formulae as follows. 



Fig. 9. An illustration of “sufficiently aligned” separating hyperplanes of 
complementary clusters I and I~ T of r. Both of the current (left) and desired 
(right) partial configurations are linearly separable by each others separating 
hyperplane, and such an alignment condition needs to be satisfied at each 
level of the subtrees rooted at I and I~ T so that the partial configurations 
x|/ and x|J- T are steered by the associated attracting fields. 


16 The recursion step at any nonsingleton cluster /EC (r) in Table IV. 8 )- 
IV. 10) updates the vector field / T)y in a bottom-up fashion, first for the 
children clusters of I and then for cluster I itself, yielding a specific order 
in which the clusters of r are visited; and such a tree traversal is formally 
referred to as the post-order tree traversal of r [81]. 
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The hierarchy-invariant vector field, f T . y , in Table IV.2) & 
IV. 3) recursively detects partial configurations, x|7 associated 
with cluster I £ G (r), that can be safely driven toward the 
goal formation in &(t) using a family of attracting controllers, 
/a : 6 (t) x (R d ) J x G (r) —> (R d )' 7 , defined in terms of the 
negated gradient field of V (x) ~- = \ ||x — y|| 2 : for any j £ J , 


f A (x, u, I)- 


f-( x i-yf). if j e 7, 

| u j , else. 


(14) 


where u £ (R d ) J is a desired (velocity) control input 
specifying the motion of complementary cluster J \ 7. 

To avoid intra-cluster collisions along the way and preserve 
(local) clustering hierarchy, for any I £ G (t) the set of 
configurations in the domain of the attracting field, f A , is 
restricted to 


I>a(7): = {x<e6(t) ||xj -Xj|| 2 > ( ri+r 3 f , Mi^j£f 

m K (x)) T sjf (x) > 0, Vfc G K, K £ Des (7, r) (15) 


where Des (7, r) is the set of descendants of 7 in t. Here, 
£ yt/ denotes the Lie derivative of a scalar-valued function / 
along a constant vector field y 7 which assigns the same vector 
y to every point in its domain, and one can simply verify that 

||xj - Xy|| 2 = (Xi - x i ) T (yi - yj ), (16) 

£ 3 j(x fe -mK(x)) T s x(x) = (yfc-m^(y)) T sjf(x) 

+ (xfe — mx (x)) T s K (y)- (17) 


Note that (16) quantifies the safety of a resulting trajectory 
of f A , and to avoid collision between any pair of disks, i 
and j, (16) should be no less than the square of sum of 
their radii, (r, + r^) 2 , as required in (15); and (17) quantifies 
the preservation of (local) clustering hierarchy and should be 
nonnegative for hierarchy invariance. Also observe that since 
a singleton cluster contains no pair of distinct indices, and has 
an empty set of descendants, the predicate in (15) is always 
true for these “leaf” node cases and we have D A (7) = ©(r) 
for any singleton cluster I £ C(r). Further, one can simply 
verify that y £ T> A (I) for any I £ G (t). 

If a partial configuration, x|7, is not contained in the domain 
of the associated attracting field, i.e. x (f_ D A (I), to avoid 
inter-cluster collisions the failure of the condition in Table 
IV.4) ensures sibling clusters, Ch (J,t), will be separated by 
a certain distance, specified as: 

T> H (I): = {x£&(T)\rik,K(x)>r k +ayk£K,K£Ch(I,T)y(m 

where (x) (7) returns the perpendicular distance of 7th 
agent to the separating hyperplane of cluster K £ G (r), and 
a > 0 is a safety margin guaranteeing that the clearance be¬ 
tween any pair of disks in complementary clusters, Ch (7, t), 
is at least 2a units. Observe that T>h (7) = @(r) for any 
singleton cluster I £ G (t) because such leaf clusters of a 
binary tree have no children, i.e. Ch (7, r) = 0. 

While the disks move in 1>h (7) based on a desired control 
(velocity) input u £ (R d )' 7 , Table IV. 10) guarantees the 
maintenance of the safety margin between children clusters 


Ch (/, r) by employing an additive repulsive field, /# : 
S(r) x (R d )’ 7 x C (r) —> (R d ) J , as follows: 

f H (x, u, I). : = Uj + 2 qj (x, u) ^ 1 n^j^n , 09) 

for all j £ K and K £ Ch (I, r); otherwise, f// (x, u, J) .: =%-, 
where aj (x, u) is a scalar valued function describing the 
strength of the repulsive field, 


Oil (x, u) : = max <^x(x) ■ ip k , K (x, u). (20) 

k£K 

KeCh(I,r) 


Here, for each individual k in cluster K £ Ch (/, r), (f) ky K (x) 
is exponential damping on the repulsion strength ip k> K (x. y), 
in which the amplitude envelop exponentially decays to zero 
after a certain safety margin j3 > a, 

/ e -(Vk,K(x)-r k -a) \ 

<t>k,K (x): = max ( -- 1 _ e - (/3 _ a) -> 0 b ( 21 ) 

i>k,K (x, u): = max (-(%,*r(x) -r k - a)-r) kiK (x), oj,(22) 
where 


f%,Jtr(x) = 


(ufc - m K (u)) T s K (x) + (x fc - m K {x)) T s K (u) 


- %,Jf(x) 


l|sjf(x)|| 

s K (x) T Sjf(u) 

l|sjf(x )|| 2 


(23) 


Note that fn (x, u, I) is well defined for any singleton cluster 
I £ C (r) and is equal to the identity map, i.e. fn (x, u, I ) = 
u, since Ch (J, r) = 0; and also observe that fn (x, u, I) = u 
for any I £ 6 (r) if the complementary clusters Ch (I, r) are 
well-separated, i.e. rj k ,K (x) > r k + /3 for all k £ K and 
K £ Ch(7, t). The latter is important to avoid the “finite 
escape time” phenomenon 17 (Proposition 14). 

Finally, Table IV. 5) guarantees that if a partial configu¬ 
ration is neither in the domain of the attracting field nor 
are its children clusters, Ch (/, r), properly separated, i.e. 
x ^ 1> A (/) U Dh (7), then the complementary clusters are 
driven apart using another repulsive field, fs ■ &(t)x (R d )' x 
C (t) —> (R d ) J , until asymptotically establishing a certain 
safety margin /3 > a: 


/s(x,u,7) 3 : = — c(x— y|7) + 2^/(x)^yp,(24) 

for all j £ K and K£ Ch (7, r); otherwise, fs (x, u, 7).: =u j, 
where the magnitude, /3j (x), of repulsion between comple¬ 
mentary clusters Ch (7, r) is given by 


/3/(x) := max max (- (r] k ,k (x) -r k -0), 0). (25) 

k&K x x ' ' 

KeGh(I,r) 


For completeness, we set fs (x, u, 7) = f A (x. u, 7) for any 
singleton cluster 7 £ G (t). 

We summarize the properties of this construction as fol¬ 
lows: 18 


17 A trajectory of a dynamical system is said to have a finite escape time 
if it escapes to infinity at a finite time [82]. 

18 This construction indeed solves Problem 1 since a flow is a retraction of 
its basin into the attractor [83]. 








ESE TECHNICAL REPORT — JULY 8, 2015 


10 


Theorem 4 The recursion of Table TV results in a well-defined 
function / r y : ©(r) —> that can be computed in 

O J| 2 ^ lime f or an y x G ©(t). F° r T £ 23 7j, the 
stratum, &{t) is positive invariant and any y £ 6(r) is 
an asymptotically stable equilibrium point of a continuous 
piecewise smooth flow arising from / Ty whose basin of 
attraction includes all of ©(r) with the exception of an empty 
interior set. 

Proof These results are proven in Appendix I according to the 
following plan. Proposition 3 establishes that the recursion in 
Table IV indeed results in a function computable in quadratic 
time. The invariance, stability, and continuous flow generating 
properties of f T y are shown using an equivalent system model 
within the sequential composition framework [10], as follows. 
Table VII defines a new recursion shown in Proposition 4 
to result in a family of continuous and piecewise smooth 
vector fields. Proposition 5 asserts that the family of domains 
associated with these fields (43) defines a (finite) open cover 
of 6(t) relative to which a selection function (Table VIII) 
induces a partition of that stratum. Proposition 6 demonstrates 
that the composition of the covering vector field family with 
the output of this partitioning function yields a new function 
that coincides exactly with the original control field defined 
in Table IV. Finally, Proposition 14, Proposition 13 and 
Proposition 15 demonstrate, respectively, the flow, positive 
invariance and stability properties of / r y , which are inherited 
from the flow, invariance and stability properties (Proposi¬ 
tion 10, Proposition 9 and Proposition 11, respectively) of 
substratum policies executed over a strictly decreasing finite 
prepares graph (Proposition 7) via their nondegenerately, real¬ 
time executed (Proposition 12) sequential composition. ■ 


C. Navigation in the Space of Binary Trees 

In principle, navigation in the adjacency graph of trees 
(Problem 2) is a trivial matter since the number of trees 
over a finite set of leaves is finite. However, in practice, the 
cardinality of trees grows super exponentially [70], 

|®Tj| = (2 | J| - 3)!! = (2 | J| - 3)(2 | J\ - 5) ... 3, (26) 

for \J\ > 2. Hence standard graph search algorithms, like 
the A* or Dijkstra’s algorithm [81], become rapidly imprac¬ 
ticable. In particular, computing the shortest path (geodesic) 
in the NNI-graph, a regular subgraph of the adjacency graph 
(Theorem 6), is NP-complete [84], 

Alternatively, we have recently developed in [85] an ef¬ 
ficient recursive procedure for navigating in the NNI graph 

'Nj = (23Tj,£jvf) towards any given binary tree r £ 23T j, 

taking the form of an abstract discrete dynamical system as 
follows: 

o k+1 = NNI (a k ,G k ) , (27a) 

G k = u T (o k ), (27b) 


where NNI (<r fc , G fe ) denotes the NNI move 19 on a k at cluster 
G k £ C (t), illustrated in Fig. 4, and u r is our NNI control 
policy returning an NNI move as summarised in Table V. 
Abusing notation, we shall denote the closed-loop dynamical 
system as 

o k+1 = g T ( a k ) : = (NNI o u) (cr k ) . (28) 

TABLE V 

The NNI Control Law 

To navigate from an arbitrary hierarchy a E 23Tj towards any selected 
desired hierarchy r E ‘BTj in the NNI-graph, the NNI control policy 
u T returns an NNI move on cr at a cluster G E C (cr), as follows: 

1) 11:7 — r. then just return the identity move, G = 0. 

2) Otherwise, 

a) Select a common cluster K E C (ojnC (r) with Ch (K, a ) f 
Ch (K, t), and let {K L ,K R } = Ch (K, r). 

b) Find a nonsingleton cluster I E C (it) with children 
{ IlJr } = Ch (7, cr) satisfying I L C K L and I R C K R . 

c) Return a proper NNI navigation move on a at grandchild G E 
Ch (I, a) selected as follows: 

i) If I~ a C K / , then return G = I R . 

ii) Else if I~ a C K R , then return G = II- 

iii) Otherwise , return an arbitrary NNI move at a child of I 

in cr; for example, G / /,. 


The NNI control law endows the NNI-graph with a directed 
edge structure whose paths all lead to r, and whose longest 
path (from the furthest possible initial hierarchy, cr £ 23Tj) is 
tightly bounded by i (| J\ — 1) (| J\ — 2) for | J\ > 2. Given 
such a goal we show in [85] that the cost of computing 
an appropriate NNI move from any other o £ 13T/ toward 
an adjacent tree at a lower value of a “discrete Lyapunov 
function” relative to that destination is O (| J|). We summarize 
such important properties of our NNI navigation algorithm as: 

Theorem 5 ([85]) The NNI control law u r (Table V) recur¬ 
sively defines a closed loop discrete dynamical system (28) in 
the NNI-graph, taking the form of a discrete transition rule, 
g T , with global attractor at t and longest trajectory of length 
O (j^| 2 ) endowed with a discrete Lyapunov function relative 
to which computing a descent direction from any cr £ 23*7 j 
requires a computation of time 0(|J|). 

D. Portal Transformations 

We now turn attention to construction of the crucial portal 
map that effects the geometric realization of the NNI-graph 
as required for Problem 3; and herein we extend our recent 
construction of the realization function. Port, in [1] for point 
particle configurations to thickened disk configurations. 

Throughout this section, the trees cr, r £ 23T j are NNI- 
adjacent (as defined in Section HI-D) and fixed, and we 
therefore take the liberty of suppressing all mention of these 
trees wherever convenient, for the sake of simplifying the 
presentation of our equations. 

9 Here, note that the NNI move at the empty cluster corresponds to the 
identity map in HTj, i.e. cr = NNI (cr, 0) for all cr £ 23Tj. Therefore, the 
notion of identity map in 23 Tj slightly extends the NNI graph by adding 
self-loops at every vertex, which is necessary for a discrete-time dynamical 
system in 23 Tj to have fixed points. 
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Since the trees <x, r are NNI-adjacent, we may apply Lemma 
1 from [85] to find common disjoint clusters A, B , 67 such that 
{A U B} = C (cr) \ C (r) and {B U 67} = C (r) \ C (a). Note 
that the triplet {A, B, 67} of the pair (<x, r) is unique. We 
call {A,B,67} the NNI-triplet of the pair (<x, r). Since a and 
r are fixed throughout this section, so will be A , B , 67 and 
P := AUBUC. 

In the construction of the portal map. Port (33), we 
restrict our attention to the portal configurations with a certain 
symmetry property, defined as: 

Definition 3 ([1]) We call x E (R d ) 1 a symmetric configura¬ 
tion associated with (a, r) if centroids of partial configurations 
x|A x| B and x|67 form an equilateral triangle, as illustrated 
in Fig. 10. The set of all symmetric configurations with respect 
to (ex, t) is denoted Sym (ex, r). 


c(x|A) 




\ / 
\ J 
S' i 

x 

r -O 


/ 

. /" 

— V.— / — 

^ A "V 

M 7 •/... \ To. A 

/ : \ 


A7 

V 


c(x|B) c(x|f?UCV \ c(x|C) 

// 

----- 

/ 1 


Fig. 10. An illustration of a symmetric configuration x S Sym (a, r), where 
the consensus ball Bq (x) of partial configuration of cluster Q S {A, B, C\ 
has a positive radius. 


An important observation about the symmetric configura¬ 
tions is: 


Lemma 1 ([1]) Let x E ©(<x) be a symmetric configuration 
in Sym(cr, r). If each partial configuration x| Q of cluster Q E 
{A, B, 67} is contained in the associated “consensus” ball 
Bq (x) — an open ball 20 centered at c (x|Q) with radius 


i-Q (x):= min -(c(x|Q) 

7 E 

£>€{Q,Pr(Q, 7 )}\{.P} 


mp , 7 


A Sp, 7 (x) 

I|sd , 7 ( x )II 


, (29) 


1) Napoleon Triangles: We recall a theorem of geom¬ 
etry describing how to create an equilateral triangle from 
an arbitrary triangle: construct, either all outer or all inner, 
equilateral triangles at the sides of a triangle in the plane 
containing the triangle, and so centroids of the constructed 
equilateral triangles form another equilateral triangle in the 
same plane, known as the “ Napoleon triangle ” [86] — see 
Fig. 11. We will refer to this construction as the Napoleon 
transformation, and we find it convenient to define the double 
outer Napoleon triangle as the equilateral triangle resulting 
from two concatenated outer Napoleon transformations of 
a triangle. Let NT : M 3d —> M. 3d denote the double outer 
Napolean transformation, see [87] for an explicit form of NT. 



Fig. 11. Outer Napoleon Triangles A a'B'C' an( ^ A a"B"C" °f A abc 
and A a'B'C'* respectively, and A a" B" C" i s referred to as the double 
outer triangle of A ABC- Note that centroids of all triangles coincides, i.e. 
c (A abc) = C i^A'B'C') = c (A A"B"C")‘ 

The NNI-triplet {A, B , 67} defines an associated trian¬ 
gle with distinct vertices for each configuration, A a,b,c ’■ 
©(<x) —y Conf (R d , [3], 0), 

A a ,b,c ( x ) : = [c (x| A) ,c (x|B) ,c (x|C)] T . (30) 

The double outer Napolean tranformation of A a,b,c ( x ) 
returns symmetric target locations for c(x|A), c(x|B) and 
c(x|C), and the corresponding displacement of c(x|P), de¬ 
noted Noff^B.c : Conf (W 1 , J, r) —> R d , is given by the 
formula 21 

Noff. 4 , B,G (x) := c (x|P) - r • NT o A a ,b,C ( x ) , (31) 


then x also supports r, i.e. x E 6(r), and so x is a portal 
configuration, x E Portal (<x, r). 

Note that for any symmetric configuration x E Sym (tx, r) the 
consensus ball of each partial configuration of cluster Q E 
{ A , B , 67} always has a nonempty interior, i.e. tq (x) > 0 [1] 
— see Fig. 10. 

In the following, we first describe how we relate any given 
triangle to an equilateral triangle using Napoleon transforma¬ 
tions, and then define our portal map. 


where F := jpy [|A| ,\B\ ,|67|] ® I d E R dx3d , and the vertices 
of the associated equilateral triangle with compensated offset 
of c (x|P) are 21 

[c a ,cb ,cc] T := NToA^^c (x) + l 3 (g>Noff.. 4 iBjC . (x). (32) 

2) Portal Maps: We now define a portal map. Port : 
@(<x) —> Portal (a, r), to be 

x , if x£ Portal (cr, r), 

(MrgoScloCtr) (x), otherwise. 


20 In a metric space ( X , d). the open ball B (x, r) centered at x with radius 
r E M>o is the set of points in X whose distance to x is less than r, i.e 
B (x, r) = {y G X \ d (x, y) < r}. 


21 Here, 1^ is the d x d identity matrix, and 1 ^ is the column vector of 
all ones. Also, <g> and • denote the Kronecker product and the standard array 
product, respectively. 
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where Ctr : 6(cr) Sym(<r, r) rigidly translates the partial 
configurations, x| A, x |B and x| C, to the new centroid loca¬ 
tions, ca, cb and cc (32), respectively, yielding a symmetric 
configuration, 

Ctr(x)--/ Xl , if * ^ P, 

° tr W • \xi-c (x|Q)+cq, if *G Q, QG{A, B, C}, ^ ’ 

It is important to observe that Ctr keeps the barycenter of 
x| P fixed, and so separating hyperplanes of the rest of clusters 
ascending and disjoint with P are kept unchanged. 

After obtaining a symmetric configuration in Sym(er, r), 
Scl : Sym (cr,r) —> Sym (cr, r) rigidly translates each partial 
configuration, x|A, x |B and x| C, to scale and fit into the 
corresponding consensus ball so that the new configuration 
simultaneously support both subtrees of cr and r rooted at P, 


to be well defined. Further, using (39), one can verify that 
c (x|Pr (I, a)) = c (x|Pr (J, a)) = c (x|Pr (J, a)) for x = 
Sep j (x), and so sa,<t (x) ^ 0 for any A £ Anc (/, a), which 
guarantees that recursive calls of Sep 7 in the computation of 
Port are always well-defined. 

We find it useful to summarize some critical properties of 
the portal map for the strata of HC2- me ans as: 

Theorem 6 The NNI-graph Af j = (BTj, £j/) is a subgraph of 
the HCo.means adjacency graph Aj = (23T/, i-e. for any 
pair (a , t) of NNI-adjacent trees in CBT j, Portal (cr, t) 

0. Further, given an edge, (cr, r) £ £j/ C £.a, a geometric 
realization via the map Port( CTjT ) : 6(cr) —> Portal (cr, r) 

(33) can be computed in quadratic, O time with the 

number of leaves, |J|. 


Scl(x): = 


x*, , if i & P 

x l+ C- (c(x|Q) — c(x|P)), ifg e {ARC } , (35) 


Proof See Appendix II-A. ■ 

VI. Numerical Simulations 


where (j £ [0, oo) is a scale parameter defined as 


<: = 


max max 
Qe{A,B,C} 


f r (x|Q) + a 
V r Q ( x ) 


- 1 . 


(36) 


Here, a > 0 is a safety margin as used in (20), and r (x|Q) 
(12) denotes the centroidal radius of partial configuration x|Q 
and tq (x) (29) is the radius of its consensus ball. Note 
that Scl preserves the symmetry of the configurations, i.e. 
centroids c (x|A), c (x|P) and c (x|C) still form an equilateral 
triangle after the mapping, and lefts the barycenter of x| P 
unchanged. 

Finally, Mrg : Sym (cr, r) —> Sym (cr, r) iteratively translates 
and merges partial configurations of common complementary 
clusters of cr and r, in a bottom-up fashion starting at P, to 
simultaneously support both hierarchies cr and r, 


Mrg (x) := Mrgp (x) , (37) 


where for any I £ {P} U Anc (P, cr) 

Mrg/(x):= 

Here, Sepj separates complementary clusters I and I~ a such 
that the clearance between every agent in / U I~ a and the 
associated separating hyperplane is at least a units (i.e. if 
x = Sep/ (x) for some x £ (K. d )' with s/ iCr (x) y^ 0, then 
Vk,K,a (x) > r k + a for any k £ K, I< £ {/, I~ a }): for any 
3 e J 


f x , if I = J, 

((Mrg Pr (p T) o Sep/) (x), otherwise. 


(38) 


Sep/ (x)j. 


Xj , if j^Pr (J,ct), 

-v- i ° , \H ” I SRr ' a jf 7 G ^• (39) 

X J+- jA |Pr(A,<T)|||sx,,(x)||’ 11 K£{I,I- a }, 


where the required amount of centroidal separation, A £ 
[0, oo), is given by 

A : = m£K max (— (rjk,K,a (x) —r* — a ), 0) . (40) 


Note that since c(x|P) = c(x|P) for any x £ ©(cr) and 
x = (Scl o Ctr) (x), we always have S/ iCT (x) y^ 0 for 
any I £ {P} U Anc (P, cr), which is required for Sep/ 


For the sake of clarity, we first illustrate the behavior of the 
hybrid system defined in Section V for the case of four disks 
moving in a two-dimensional ambient space . 22 

In order to visualize in this simple setting the most compli¬ 
cated instance of collision-free navigation and observe maxi¬ 
mal number of transitions between local controllers, we pick 
the initial, x £ ©(it), and desired configurations, x* £ ©(74), 
where disks are placed on the horizontal axis and left-to- 
right ordering of their labels are (1,2,3,4) and (3*, 1*, 4*, 2*), 
respectively, and their corresponding clustering trees are T\ £ 
25T[4] and T 4 £ ®T[ 4 ], see Fig. 12. 

The resultant trajectory of each disk following the hybrid 
navigation planner in Section V, the relative distance between 
each pair of disks and the sequence of trees associated with 
visited hierarchical strata are shown in Fig. 12. Here, the disks 
start following the local controller associated with n until they 
enter in finite time the domain of the following local controller 
associated with T 2 at x c £ S(n) fl ©(T 2 ) — shown by cyan 
dots in Fig. 12. After a finite time navigating in ©(72) and 
©(73), respectively, the group enters the domain of the goal 
controller / T4)X * (Table IV) at x r £ ©(73) fl ©(74) — shown 
by red dots in Fig. 12, and / T4 . x * asymptotically steers the 
disks to the desired configuration x* £ ©(74). Finally, note 
that the total number of binary trees over four leaves is 15; 
however, our hybrid navigation planner reactively deploys only 
4 of them. 

We now consider a similar, but slightly more complicated 
setting: a group of six disks in a plane where agents are 
initially placed evenly on the horizontal axes and switch their 
positions at the destination as shown in Fig. 13(a), which is 
also used in [17] as an example of complicated multi-agent 
arrangements. While steering the disks towards the goal, the 
hybrid navigation planner automatically deploys only 6 local 
controllers out of the family of 945 local controllers. The time 
evolution of the disk is illustrated in Fig. 13(a). 

22 For all simulations we consider unit disks moving in an ambient plane, 
i.e. Vj = 1 for all j S J, and we set a = 0.2 and (3 = 1; and all simulations 
are obtained through numerical integration of the hybrid dynamics generated 
by the HNC algorithm (Table III) using the ode45 function of MATLAB. 
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Fig. 12. An illustrative navigation trajectory of the hybrid dynamics generated by the HNC algorithm for 4 disks in a planar ambient space. Disks are placed 
on the horizontal axis for both the initial and desired configurations in different orders, from left to right (1,2, 3,4) and (3*, 1*, 4*, 2*) at the start and 
goal, respectively, (a) The sequence of trees associated with deployed local controllers during the execution of the hybrid navigation controller, (b) Centroidal 
trajectory of each disk colored according the active local controller, where x c E ©(ri) D ©(T 2 ), x g E ©(T 2 ) fl ©( 7 * 3 ) and x r E ©( 73 ) D ©( 7 - 4 ) shown by 
cyan, green and red dots, respectively, are portal configurations, (c) Space-time curve of disks (d) Pairwise distances between disks. 



Fig. 13. Example trajectories of the hybrid vector field planner for (a) 6, (b) 8 and (c) 16 disks in a planar ambient space, (top) Trajectory and (bottom) 
state-time curve of each disk. Each colored time interval demonstrates the execution duration of an activated local controller. Dots correspond to the portal 
configurations where transitions between local controllers occur at. 


Finally, to demonstrate the efficiency of the deployment 
policy of our hybrid planner, we separately consider groups of 
8 and 16 disks in an ambient plane, illustrated in Fig. 13. The 
eight disks are initially located at the corner of two squares 
whose centroids coincide and the perimeter of one is twice 
of the perimeter of the other. At the destination, disks switch 
their locations as illustrated in Fig. 13(b). For sixteen disk 
case, disks are initially placed at the vertices of a 4 by 4 grid, 
and their task is to switch their location as illustrated in Fig. 
13(c). Although there are a large number of local controllers 
for the case of groups of 8 and 16 disks (|25T[ 8 ]| > 10 5 
and |lB0'[ 1 6] | > 6 x 10 15 ), our hybrid navigation planner only 


deploys 9 and 19 local controllers, respectively. 

The number of potentially available local controllers for 
a group of n disks (26) grows super exponentially with n. 
On the other hand, if agents have perfect sensing and actua¬ 
tion modelled as in the present paper, the hybrid navigation 
planner automatically deploys at most i (n — 1) (n — 2) local 
controllers [85], illustrating the computational efficiency of our 
construction. 

VII. Conclusion 

In this paper, we introduce a novel application of clus¬ 
tering to the problem of coordinated robot navigation. The 
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notion of hierarchical clustering offers a natural abstraction 
for ensemble task encoding and control in terms of precise yet 
flexible organizational specifications at different resolutions. 
Based on this new abstraction, we propose a provably correct 
generic hierarchical navigation framework for collision-free 
motion design towards any given destination via a sequence 
of hierarchy preserving controllers. For the 2-means divisive 
hierarchical clustering [69], based on a topological characteri¬ 
zation of the underlying space, we present a centralized online 
(completely reactive) and computationally efficient instance of 
our hierarchical navigation framework for disk-shaped robots, 
which generalizes to an arbitrary number of disks and ambient 
space dimension. 

Specifically, matching the component problem statements 
of Section IV to their subsequent resolution: we address 
Problem 1 in Theorem 4 (guaranteeing that the construction of 
Table IV results in a hierarchy invariant vector field planner); 
we address Problem 2 in Theorem 5 (guaranteeing that the 
construction of Table V results in a reactive strategy that 
finds, given any non-goal tree, an edge in the graph of all 
hierarchies leading to a new tree that is closer to the desired 
goal hierarchy); and we address Problem 3 in Theorem 6 
(providing a geometric realization in the configuration space 
of the combinatorial edge toward the physical goal). The 
efficacy of this overarching strategy is guaranteed by Theorem 
1 (proving the correctness of these problems steps and their 
resolutions as presented in Table III). 

Work now in progress targets more practical settings in 
the field of robotics including navigating around obstacles 
in compact spaces and a distributed implementation of our 
navigation framework. We are also exploring a number of ap¬ 
plication settings for hierarchical formation specification and 
control including problems of perception, perceptual servoing, 
anomaly detection and automated exploration and various 
problems of multi-agent coordination. 

In the longer term, especially when the scalability and 
efficiency of hierarchical protocols in sensor networks for 
information routing and aggregation is of concern [88], these 
methods suggest a promising unifying framework to simulta¬ 
neously handle control, communication and information ag¬ 
gregation (fusion) in multi-agent systems. 

Appendix I 

Properties of The Hierarchy Invariant Vector 
Field 

Although the recursive definition of the hierarchy preserving 
navigation policy, / Ty , in Table IV expresses an efficient 
encoding of intra-cluster and inter-cluster interactions and 
dependencies of individuals, which we suspect will prove to 
have value for distributed settings, it yields a discontinuous 
vector field complicating the qualitative (existence, unique¬ 
ness, invariance and stability) analysis, as anticipated from the 
proof structure of Theorem 4 in Table VI. We find it convenient 
to proceed instead by developing an alternative, equivalent 
representation of this vector field. Namely, we introduce a 
family of continuous and piecewise smooth covering vector 
fields whose application over a partition (derived from their 


TABLE VI 

The Proof Structure of Theorem 4 : Logical Dependencies 

• Proposition 3 (Quadratic Time Function) [I, p.14 4= II-B, p. 19] 

• Proposition 4 (Continuous & Piecewise Smooth) [I-A, p.15 4= II-C,p.20] 

- Lemma 5 (Child Partition Block) [I-F, p. 18] 

• Proposition 5 (Domain Covering Induced Partition)[I-A,p.l54=II-D,p.20] 

• Proposition 6 (Equivalent Vector Field) [I-A, p.16 4= II-E, p.21] 

• Proposition 13 (Stratum Positive Invariance) [I-D, p. 17] 

- Recalls Proposition 5, Proposition 6 

- Proposition 9 (Substratum Positive Invariance) [I-C, p.17 4= ILL p.22] 

* Lemma 7 (Invariance - Base Case 1) [ILL p.22 4= H-M, p.24] 

* Lemma 8 (Invariance - Base Case 2) [ILI, p.22 4= II-N, p.25] 

* Lemma 9 (Invariance - Recursion) [IIT, p.22 4= II-O, p.25] 

• Proposition 14 (Stratum Existence & Uniqueness) [I-D, p. 17] 

- Recalls Proposition 5, Proposition 6 

- Proposition 10 (SubstratumExistenceUniqueness)[I-C,p. 174= II-H, p.22] 

* Recalls Proposition 4, Proposition 9. 

* Lemma 3 (Relative Centroidal Dynamics) [I-F, p. 18 4= ILK, p. 23] 

* Lemma 4 (Configuration Bound Radius) [I-F, p.4 4= ILL, p.24] 

• Recalls Lemma 3. 

- Proposition 15 (Stratum Stability) [I-D, p. 17] 

* Recalls Proposition 6, Proposition 9. 

* Proposition 8 (Substratum Policy Selection) [I-C, p. 17 4= H-G, p.21] 

■ Recalls Proposition 5. 

■ Lemma 6 (Partition Refinement) [I-F, p. 19] 

* Proposition 11 (Finite Time Prepares Relation) [I-C, p.17 4= II-J, p.23] 

■ Lemma 10 (Case (i) in Definition 5) [II-J, p.23 4= II P, p.II-P] 

■ Lemma 11 (Case (ii) in Definition 5) [II-J, p.23 4= II-Q, p.II-Q] 

■ Lemma 12 (Case (iii) in Definition 5) [II-J, p.23 4= II-R, p.II-R] 

• Proposition 7 (Substratum Prepares Graph) [I-B, p.17 4= ILF, p.21] 

- Recalls Lemma 5. 

• Proposition 12 (Nondegenerate Execution) [I-C, p.17] 

- Recalls Proposition 8, Proposition 9. 

- Lemma 2 (Closed Substratum Domain) [I-C, p.18] 


covering domains) of the stratum yields a continuous piece- 
wise smooth flow (identical to that generated by the original 
construction) which is considerably easier to analyze because 
it admits an interpretation as a sequential composition [10] 
over the covering family. 

We find it useful to first observe that the original construc¬ 
tion yields a well defined and effectively computable function. 

Proposition 3 The recursion in Table TV results in a well 
defined function, f Ty : © (r) —> (R d ) , that can be computed 
for each configuration x G ©(r) in O (| J|"^ time. 

Proof See Appendix II-B. ■ 

A. An Equivalent System Model 

Key for understanding the hierarchy preserving navigation 
policy, f T ,y , i n Table IV is the observation that for any 
configuration x G ©(r) the list of visited clusters of r 
satisfying base conditions during the recursive computation 
of f t, y defines a partition 3 of J compatible with r, i.e. 

3 c 6 (t). 23 

Now observe, depending on which base condition holds 
(Table IV. 2) or Table IV. 4)), every block I of partition 

23 Note that the recursions in Table IV and Table VIII have the same base 
and recursion conditions, and the recursion in Table VIII returns the list of 
clusters satisfying base conditions, which defines a partition of J (Proposition 
4). Hence, using the relation between these recursions in Proposition 6, one 
can conclude this observation. 



ESE TECHNICAL REPORT — JULY 8, 2015 


15 


TABLE VII 

Local Control Policies in a Hierarchical Stratum 


Let d be a partition of J with 3 C G (r), and b = (b/)j e ^ E 

{- 

-1, +1}". For any desired y E ©(t), supporting r E D7j, and any 

initial x E D (3, b) (43), the local control policy, /i#,b • D (3, b) —»• 

(R d ) J 




ha,b(x) :=^3,b(x, 0, J), 

is 

recursively computed using the post-order traversal of r starting at 

the root cluster J with the zero control input 0 E (R d ) as follows: 

for any u G (R d ) J and 1 G V T (3) (45), 


i) 

function u = hg b ( x > u j 


2) 

if / e a. 

a> 

3) 

if bj = +1 

c§ 

U < 

4) 

u 5— /a (x, u, I) (14), % Attracting Field 

<D 

5) 

else 

c3 

CQ 

6) 

u <— fs (x, u, I) (24), % Split Separation Field 


7) 

end 


8) 

else 

C 

9) 

{Il,Ir} 5— Ch(7,r), 

% , 

10) 

u_l <— /ig,b (x, u, If), % Recursion for Left Child 

b 

<D 

ID 

UflJ - (za,b(x,UL,Tfl), % Recursion for Right Child 


12) 

u J - Ih (x, ur, I) (19), % Spin Preserving Field 


[l3) 

end 


14) 

return u 


3, associated with any fixed configuration x £ D a ( I ) U 
(&(t) \ T>h (I)), can be associated with a binary scalar 
b/ (x) £ {—1, +1} such that 24 


bz (x) 


+1 , if x G T>a (I) , 

-1 , if x £ (/) U , 


(41) 


where T>a ( I ) and Dh (I) are defined as in (15) and (18), 
respectively. We will use this configuration space labeling 
scheme to recast the hierarchy preserving control policy / T . y 
as an online sequential composition of a family of continuous 
and piecewise smooth local controllers indexed by partitions of 
J compatible with r and associated binary vectors as follows. 

A partition 3 of J is said to be compatible with r £ 13Tj 
if and only if 3 C 6 (t), and denote by (P j (t) the set of 
partitions of J compatible with r. Accordingly, define SCPj (r) 
to be the set of substratum policy indices, 


SOMr) := {(0,b) 


3e?j(r),be {—i,+i} a }. 


(42) 


For any partition 3 £ 3Pj (t) of J and b := (b i) Ie g £ 
{—1, +1} 3 , the domain D (3, b) of a local control policy /ig.b. 
presented in Table VII, is defined to be 


D(0,b) := p| (D B (J,bj) n f) (43) 

IG3 KG Anc ( I,t) 


where the set of configurations satisfying the base condition 
associated with cluster I of 3 and binary scalar b/ is given by 


(/,b/) 


f D a (I), if b/ = +1, 
\ &(t) , if b/ = -1, 


(44) 


24 Observe from Table IV that any configuration x E ©(r) satisfies 
a base condition (Table IV. 2) or Table IV.4)) at cluster /EC (r) if 
x E D A (-0 U (©(r) \ Dh (/)). Also note that we have Da (/) U 
(6(r) \ D h (/)) = D a (/) U (6(r) \ (D A (/) U D H (/))), and D A (/) 
and ©(r) \ ( D a (/) U Dh (/)) are disjoint. 


TABLE VIII 

Policy Selection Algorithm 


For 

any 

initial x E 0(r) and desired y E ©("T"), supporting r E STj, 

the policy selection algorithm, p : ©(r) —> STj (r), 



p(x) : = p(x, J), 

recursively generates a local policy index in STj (r) (42) using the 

post-order traversal of r starting at the root cluster J as follows: for 

any / G C (r). 


i) 

function (5, b) = p (x, /) 


r 2) 

if x G T)a (I) (15), 


3) 


C§ 

4) 

b i — +1, 

(U 

5) 

else if x ^ T>h (/) (18), 

cd 

CQ 

6) 



7) 

b -t - 1. 


8) 

else 


9) 

{IlDr} Ch(7,r), 

_o 

10) 

0L,i> L ) <-p(x,I L ), 

3 

O 

11) 

0R, bfi) J— P ( x , 7r), 

oc 

12) 

i <— u 5 r, 


13) 

b G- bi||b fl , 26 


L14) 

end 


15) 

return (j, b) 


and all ancestors K £ Anc (/, t) of I in r satisfy the 
recursion condition of having properly separated children 
clusters described by Dh (K) (18). Accordingly, let V T (0) 
denote the set of clusters of r visited during the recursive 
computation of hg,b in Table VII, 

V T (0):={ifee(r)|lfDI,Ie0}. (45) 

Note that J £ V T (0) since 0 is a partition of the root cluster 
J and any block I £ 3 satisfies I C J. 

Observe that each local control policy hg j-, is a recursive 
composition of continuous functions of x, so it is continuous: 

Proposition 4 The recursion in Table VII results in a contin¬ 
uous and piecewise smooth function 25 , hg b : G(r) —> (]R d ) . 

Proof See Appendix II-C. ■ 

To conclude our introduction of the family of covering fields 
in Table VII, we now observe that the vector field / T . y in Table 
IV is an online concatenation of continuous local controllers, 
/ig b, of Table VII using a policy selection method described 
in Table VIII, summarized as: 

Proposition 5 For any given configuration x £ G(t) the 
policy selection algorithm p in Table VIII always returns a 
valid policy index, (0,b) = p(x), in S(Pj (r) (42) such that 
the domain D (0, b) (43) of the associated local control policy 
/ig b (Table VII) contains x, i.e. 

x £ (D op) (x). (46) 


25 Note that if / : U —> R m is continuous and piecewise smooth on an 
open set U C R”, then it is locally Lipschitz on U [89], 

26 Here, p||q denotes the concatenation of vectors p and q. That is to say, 
let X, Y be two sets and A, B be two finite sets of coordinate indices, then 
for any p G X A and q G Y B we say r G X A X Y B is the concatenation 
of p and q, denoted by r = p||q, if and only if r a = p a and ry, = (p, for 
all a G A and b G B. 
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Proof See Appendix II-D. ■ 

Proposition 6 For any given configuration x £ @(r), the 
vector field / T)X (Table IV) and the local control policy 
b (x) (Table VII) selected as (<J, b) = p (x) (Table VIII) 
generate the same control (velocity) inputs, i.e. 

fr , y (x) = /ip(x) (x). (47) 

Proof See Appendix II-E. ■ 

Since the hierarchy invariant field f T y is defined for entire 
6 (t), it is useful to remark that the domains, D(3, b) (43), 
of local control policies, hgy,, define a cover of ©(r) indexed 
by partitions of J compatible with t and associated binary 
vectors. 

B. Online Sequential Composition of Substratum Policies 

We now briefly describe the logic behind online sequential 
composition [ 10] of substratum policies. 

To characterize our policy selection strategy, we first define 
a priority measure 27 for each local controller /t-jb associated 
with a partition 3 £ CPj (r) of J and a binary vector b £ 
{—1, +1} 3 to be 

priority (0, b) := |/| 2 . (48) 

/eg 

Note that the maximum and minimum of the priority measure 
is attained at the coarsest partition {J} of J, and bj = +1 
and bj = — 1, respectively, 

priority ({J} ,+l) = | J| 2 , (49a) 

priority ({J} , -1) = - | J| 2 . (49b) 

Accordingly, we shall refer to the local control policy with 
index ({ J} , +1) as the goal policy since it has the highest 
priority and asymptotically steers all configurations in its 
domain CD ({J} , +1)(43) to y following the negated gradient 
of V (x) = 4 11x - y||, i.e. for any x £ CD ({ Jj , +1) 

h{jy,+i (x) = -VV(x) = -(x- y). (50) 

Note that since the root cluster J has no ancestor, i.e. 
Anc (J, t) = 0, by definition (43), D({J},+1) = T>a{J), 
and T>a (J) (15) contains the goal configuration y. 

We now introduce an abstract connection between local 
policies for high-level planning: 

Definition 4 Let (3, b), (3', b') £ SCPj (r) be two distinct 
substratum policy indices. Then /ig v, is said to prepare hg\\y 
if and only if all trajectories of hg b starting in its domain 
CD ( 3 , b), possibly excluding a set of measure zero, reach 
CD(3',b') in finite time. 28 

Accordingly, define the prepares graph IPS = 
(SCPj(r),£ o>g) to have vertex set STj (r)(42) with a 
policy index (3, b) G STj (r) connected to another policy 

27 In the general past literature, such a priority assignment of local con¬ 
trollers is done using backchaining of the prepares graph in an offline manner 
[10]. 

28 Here, we slightly relax the original definition of the prepares relation 
in [10] by not requiring the knowledge of goal sets, globally asymptotically 
stable states, of local control policies in advance. 


index (3 / ,b / ) by a directed edge in £yg if and only if hgt, 
prepares hg> b'- 

Although, the prepares graph CPS is the most critical compo¬ 
nent of the sequential composition framework [ 1 0] defining a 
discrete abstraction of continuous control policies, the expo¬ 
nentially growing cardinality of substratum policies, discussed 
in Appendix I-E, and the lack of an explicit characterization 
of globally asymptotically stable configurations of substratum 
policies make it usually difficult to compute the complete 
prepares graph. 

Alternatively, we introduce a computationally efficient and 
recursively constructed graph of substratum policies that is 
nicely compatible with our needs, yielding a subgraph of 
the prepares graph, where every policy index is connected to 
the goal policy index ({«/}, +1) through a directed path, as 
follows. 

Definition 5 Let CPS = (SCPj (r), £pg) be a graph with vertex 
list SCPj (r), and a policy index ( 3 , b) £ SCPj (r) that is 
connected to another policy index (3', b') £ SCPj (r) by a 
directed edge in £yg if and only if at least one of the following 
properties holds: 29 

(i) (Complement) There exists a singleton cluster I £ 3 such 
that bj = —1, and 3' = 3 and b' £ {—1,+1} 3 with 
bj = +1 and b^, = bj) for all D £ 3 \ {/}. 

(ii) (Split) There exists a nonsingleton cluster / £ 3 such 
that bj = —1, and 3' = 3 \ {/} U Ch (/, r) and b' £ 
{—1,+1} 3 with b' K = —1 for all K £ Ch (/, r) and 
bj) = b_o for all D £ 3 \ Ch (I, r). 

(iii) (Merge) There exists a nonsingleton cluster / £ 6 (r) such 
that Ch (/, t) C 3 and b# = +1 for all K £ Ch (J, r), 
and 3' = 3 \ Ch (/, r) U {/} and b' £ {—1, + l} 3 with 
h'j = +1 and b^, = bj? for all D £ 3 \ Ch (/, r). 

Note that, since 3 is compatible with r, i.e. a CE(r), if |3| > 
1, then there exists a cluster I £ C (r) such that Ch (J, r) C 3 
(Lemma 5). Hence, for any policy index (3,b) ^ ({./} ,+l) 
there always exists a policy index (3',b') (0, b) satisfying 

one of these conditions, (i)-(iii) above. Thus, the out-degree 
of a policy index (0,b) ^ ({J} ,+1) in CPS is at least one, 
whereas the goal policy index ({«/} , +1) in CPS has an out- 
degree of zero. We summarize some important properties of 
CPS as follows: 

Proposition 7 The graph CPS = (SCPj (r), £yg), as defined 
in Definition 5, is an acyclic subgraph of the prepares graph 
CPS = (SCPj (r),£a» g ) (Definition 4) such that all policy 
indices in SCPj (r) are connected to the goal policy index 
({J},+1) through directed paths in £j>g, of length at most 
O hops, along which priority (48) is strictly increas¬ 

ing, i.e. for any ( (3, b), ( 3 b')) £ £o>g 

priority^, b') > priority (3, b). (51) 

Proof See Appendix II-F. ■ 

29 One may think of these conditions as restructuring operations of policy 
indices by merging/splitting of partition blocks and/or alternating binary index 
values, like NNI moves of trees in Section III-D. 
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Although a given local policy can prepare more than one 
potential successor (i.e. higher priority), our policy selection 
method chooses the one with the strictly highest priority: 

Proposition 8 For any given x £ ©(t) the policy selection 
method, p, in Table VIII always returns the index of a 
local controller with the maximum priority among all local 
controllers whose domain contains x, 

p(x) = arg max priority^', b'). (52) 

0',b')esa>j(T) 

xeD(g',b') 

and all the other available local controllers have strictly lower 
priorities. 

Proof See Appendix II-G. ■ 

C. Qualitative Properties of Substratum Policies 

We now list important qualitative (existence, uniqueness, 
invariance and stability) properties of the substratum control 
policies of Table VII. Let 0 be a partition of J compatible 

with r, i.e. 3 C G (t), and b is a binary vector in {—1, l} 3 . 

Proposition 9 The domain, T>(3,h) (43), of a substratum 
policy, hg(Table VII), is positive invariant. 

Proof See Appendix II-I. ■ 

Proposition 10 (Substratum Existence and Uniqueness) The 
vector field hg b (Table VII) is locally Lipschitz in © (r); and 
for any initial x £ T>(3,b) C ©(r) there always exists a 
compact (bounded and closed) subset W of D (3, b) (43) such 
that all trajectories of hg t b starting at x remain in W for all 
future time. 

Therefore, there is a unique continuous and piecewise 
smooth flow of hg b in D (3, b) that is defined for all future 
time. 

Proof See Appendix II-H. ■ 

Proposition 11 (Finite Time Prepares Relation) Each local 
control policy, hgy,, with the exception of the goal controller 
h{j},+i, steers (almost) all configurations in its domain, 
D ( 3 , b), to the domain, T> (3'. b'), of another local controller, 
hg',b', at a higher priority (48) in finite time. 

Proof See Appendix II-J. ■ 

Proposition 12 (Nonzero Execution Time) Let x * be a trajec¬ 
tory of the local control policy hgb starting at x° £ D (3, b) 
such that p (x°) = (3,b). 

Then the local controller is guaranteed to steers the group 
for a nonzero time until reaching the domain of a local 
controller at a higher priority (48), i.e. 

inf {t > 0\p (x 4 ) 7 ^ (3, b)} > 0. (53) 

Proof Recall that for any configuration x £ ©(r) the policy 
selection method in Table VIII always yields the index of 
the local controller with the highest priority among all local 
controllers whose domains contain x (Proposition 8 ). Hence, 
since the initial configuration x° is not included in the domain 
of any other local controller with a higher priority than 


priority( 3 , b) and domains of local controllers are closed 
relative to ©(t) (Lemma 2), there always exists an open set 
around x° which does not intersect with the domain of any 
local controller at a higher priority than priority^, b). Thus, 
since its domain is positively invariant (Proposition 9), hgy, 
is guaranteed to steer the configuration in the intersection of 
this open set and D (3, b) for a nonzero time. ■ 

D. Qualitative Properties of Stratum Policies 

We now proceed with some important qualitative (existence, 
uniqueness, invariance and stability) properties of the hierar¬ 
chy preserving navigation policy of Table IV. 

Proposition 13 The stratum 6 (t) is positive invariant under 
the hierarchy-invariant control policy, / Ty (Table TV). 

Proof Recall that the domains, D (43), of local control 
policies in Table VII define a cover of ©(r) (Proposition 
5) each of whose elements is positively invariant under the 
flow of the associated local policy (Proposition 9). Thus, the 
result follows since the hierarchy preserving vector field f Ty 
is equivalent to online sequential composition of local control 
policies of Table VII based on the policy selection algorithm 
in Table VIII (Proposition 6 ). ■ 

Proposition 14 (Stratum Existence and Uniqueness) The 
hierarchy invariance control policy, / r y (Table TV), has a 
unique, continuous and piecewise smooth flow, p 1 , in &(t), 
defined for all t > 0. 

Proof Recall from Proposition 6 that f T ,y is equivalent to on¬ 
line sequential composition of a family of substratum policies 
which have unique, continuous and piecewise smooth flows, 
defined for all t > 0 , in their positive invariant domains 
(Proposition 10). Since their domains define a finite closed 
cover of ©(r) (Proposition 5), the unique, continuous and 
piecewise flow of f Ty is constructed by piecing together 
trajectories of these substratum policies. ■ 

Proposition 15 Any y £ 6 (r) is an asymptotically stable 
equilibrium point of the hierarchy-invariant control policy, 
f T) y (Table IV), whose basin of attraction includes ©(r), 
except a set of measure zero. 

Proof Using the equivalence (Proposition 6 ) of the hierar¬ 
chy preserving field / Tiy and the sequential composition of 
substratum control policies of Table VII based on the policy 
selection method in Table VIII, the result can be obtained as 
follows. 

Since priority (48) is an integer-valued function with 
bounded range (49), using Proposition 8 and Proposition 11, 
one can conclude that the disks starting at almost any con¬ 
figuration in ©(r) reach the domain © ({J} , + 1 ) of the goal 
policy h{j + 1 } in finite time after visiting at most O (j^| 2 ') 
of other local control policies. Note that y £ D ({ J} , +1). 
Then, the goal policy /i{jj +1 

h {J},+1 ( x ) = —||x-y ||2 = - (x-y), (54) 

asymptotically steers all configuration in ©({</},+ 1 ) to y 
while keeping its domain of attraction 1) 4 (J) positively 
invariant (Proposition 9), which completes the proof ■ 
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E. On the Cardinality of Substratum Policies 

To gain an appreciation for the computational efficiency of 
hierarchy preserving vector field in Table IV, we find it useful 
to have a brief discussion without proofs on the cardinality of 
the family of local control policies of Table VII. The number 
of partitions CP j (r) of J 30 compatible with a cluster hierarchy 
r £ CBTj is recursively given by 31 

|CPj (t)| =1 + |CPj (t l )||CPj (t p )|, (56) 

where 77 ,, r p denote the left and right subtrees of r, respec¬ 
tively. For any caterpillar tree 32 a £ CBT j, |CPj(cr)| = |J| 
since one of two subtrees of a is always one-leaf tree. On the 
other hand, for a balanced tree 7 £ CBT/ the cardinality of 
partitions of J compatible with 7 grows exponentially , 33 

nr\J\ 

W' 71 < |CP^(7)| < Iv! - (57) 

for |J| = 2 k , k £ N+ = {1,2,3,...}; for example, 
(P[ 2 ] (7) | = 2, |CP[ 4 ] (7)1 = 5, JCP [8 ] ( 7 )| = 26 and 
CP[i 6 ] ( 7 )! = 677. In addition to a partition 3 of J compatible 
with r, every local control policy h'y\, is indexed by a binary 
variable of size | 3 | with a possible choice of 2^1 values. 
Therefore, the number of local control policies hg,b grows 
exponentially with the group size, | J\. 

F. A Set of Useful Observations on Substratum Policies 

Here we introduce a set of useful lemmas that constitute 
building blocks for proving some qualitative properties of 

30 The number of partitions of a set with n elements is given by the Bell 
number, B n , recursively defined as: for any n E N [90] 


substratum policies presented in Appendix I-C. Let 3 be a 
partition of J compatible with r, i.e. 3 C C (r), and b is a 
binary vector in {— 1 , + 1 } 3 . 

Lemma 2 The domain, T) (3, b) (43), of each substratum 
policy, Lg b. is closed relative to ©(r). 

Proof Using the continuity of functions 34 in the predicates 
used to define them, one can conclude that for any I £ C (r) 
sets Da ( I ) (15) and Dr (I) (18) are closed relative to &(t). 
Hence, since the intersection of arbitrary many closed sets are 
closed [91], the domain D (3, b) (43) of each local controller 
Lg b is closed relative to ©(r). ■ 

A critical observation used for bounding the centroidal 
configuration radius (Lemma 4) and the range of a trajectory 
of a substratum policy (Proposition 10) is: 

Lemma 3 (Relative Centroidal Dynamics) Let x £ ©(r) and 


u = h 3 . h (x). 


(62) 


Then, the centroidal dynamics of any cluster I £ V T (3) 
(45) visited during recursive computation of hg,b (Table VII), 
except the root J, satisfies 35 


c (u| I) = -c (x-y|7) + 2 ap (x, v P ) 


|/- T | S/(x) 


p I IMx)|| 

+ c(u|P)+c(x-y|P), (63) 


for some v P £ (R d )' 7 associated with parent cluster P = 
Pr (J, t); whereas we have for the root cluster J 


c( u |J) =-c(x-y|J). (64) 


n 

B n+ 1 = ]T Q Bk , (55) 

k =0 

where Bo = 1. The Bell number, B n , grows super exponentially with the 
set size, n; however, in our case we require partitions of J to be compatible 
with r and this restricts the growth of number of such partitions of J to at 
most exponential with \J\, depending on the structure of r. 

31 Let { Jl , Jr} = Ch (J,r) be the root split of r, and tl and tr are 
the associated subtrees of r rooted at Jl and Jr, respectively. Then, any 
partition of J compatible with r, except the trivial partition {J}, can be 
written as the union of a partition of Jl compatible with tl and a partition 
of Jr compatible with tr. Hence, one can conclude the recursion in (56). 

32 A caterpillar tree is a rooted tree in which at most one of the children of 
every interior cluster is nonsingleton. 

33 Let F n denote the number of partitions of [n] = {1, 2,..., n\ compati¬ 
ble with a balanced rooted binary tree with n leaves, where n = 2 k for some 
k E N-|_, and by (56) it satisfies 

F 2n = l + Fl (58) 

subject to the base condition F 2 = 2. Define G n and H n , for n = 2 k and 

k E N+, to be, respectively, 

G 2n = Gl and H 2n = \nl (59) 

where G 2 = H 2 = 2. Note that Gn = and H n = | f° r 71 = 

and k E N_|_. Now observe that for any n = 2 k and k E N_|_ 

Gn<F n <H n , (60) 

and so 


Proof See Appendix II-K. ■ 

Lemma 4 (Upper Bound on Configuration Radius) Let x f 
denote a trajectory of h^,b (Table VII) starting at any initial 
x° e D (3, b) (43) for t > 0. 

Then, the centroidal configuration radius, r(x*| J) (12), 
is bounded above for all t > 0 by a certain finite value, 
R (x°,y), depending on x° and y, i.e. 

r ( x *|T) < R (x°, y) < oo, Vf > 0. (65) 

Proof See Appendix II-L. ■ 

Lemma 5 If 3 is not the trivial partition, i.e. |3| > 1, then 
there always exists a cluster I £ 6 (r) such that Ch (J, r) C 3- 

Proof Define the depth of cluster I £ 6 (r) in r to be the 
number of its ancestors, |Anc(I, r)|. 

Let I\ £ 3 be a cluster in 3 with the maximal depth, i.e. 

|Anc(/T,r)| = arg max | Anc (D, r)|. (66) 

De3 

Then, we now show that K~ T is also in 3. and so I = 
Pr ( K , r) satisfies the lemma. 

34 A function / : X —J Y between two topological spaces, X and Y, is 
continuous if the inverse image of every open subset of Y of / is an open 
subset of X [91], 

35 Here, for any I S C (r) we use c : (R d ) 7 —J R d (8). 


( 61 ) 
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Proof by a contradiction. Suppose that K~ T is not in 3- 
Since 3 is a partition of J compatible with r, then some de¬ 
scendant D £ Des ( K~ T , t) is in 3. Note that | Anc (K , r)| = 
|Anc(A' _r )| < |Anc(D, r)|, which contradicts (66). ■ 

Lemma 6 Let 3 and 3' be two distinct partitions of J 
compatible with t, i.e. 3 3' C 6 (r). Then, at least one 

of the followings always holds 

(i) (3 Partially Refines 3') There exists a cluster K' £ 3' 
with a nontrivial partition %' such that %' C 3- 

(ii) (3' Partially Refines 3) There exists a cluster K £ 3 with 
a nontrivial partition 3C such that X C 3'■ 

Proof For any j £ J , let 3 (j) denote the unique element of 
3 containing j. 

Since 3 ^ 3', let K' £ 3’ \ 3 be an unshared cluster. Since 
both 3 and 3' are partitions of J compatible with r, we have 
either 3(k') C K' or 3(k') D A' for all k' £ K'. If 3(k') C 
K' for all k' £ A', then X' = {3(k')\k' £ A''} defines a 
partition of A' and we obtain Lemma 6.(i). Otherwise, by 
symmetry, we have Lemma 6.(ii). Thus, the lemma follows. 


Appendix II 
Proofs 

A. Proof of Theorem 6 

Proof To prove the first part of the result, we shall consider 
Portg- T- as a mapping from ©(cr) to (R d )' and verify that 
Port^T- (6(a)) C Portal (cr, r). 

By definition, the restriction of Port CT T to Portal (a, t) is 
the identity map on Portal (a, t). Hence, we only need to 
show that Porter.,- (©(cr) \ Portal (cr, r)) C Portal (cr, r). 

Let y = Ctr (x) and z = Scl (y) be intermediate con¬ 
figurations during the portal transformation of a configuration 
x £ 6(a) \ Portal (cr, r) into w = Mrg (z) = Port (x). 

First, recall that rigid transformations and scaling of partial 
configurations preserve their clustering structure [29], Hence, 
the common subtrees of a and r rooted at A, B and C are 
preserved after each transformation by Ctr (34), Scl (35) and 
Mrg (37). 

Second, each partial configuration of the symmetric con¬ 
figuration y £ Sym (cr, r) associated with (cr, r) is properly 
translated by Scl (35) so that each of them lies in the 
corresponding consensus ball, i.e. r (z\Q) < vq (z) for all 
Q £ (A, B,C). Hence, the partial configuration z| P supports 
both of the subtrees of a and r rooted at P. 

Finally, if P = J, then the result simply follows since 
z = w £ Portal (cr, r). Otherwise, for every I £ {P} U 
Anc (P,a), Mrg (37) iteratively separates the common com¬ 
plementary clusters I and I~ a of cr and r, in a bottom up 
fashion starting at cluster P, to support the subtrees of a and 
r rooted at Pr (I, a). Note that in the base case z supports 
both of the subtrees of a and r rooted at P and P~ a \ and at 
the termination at cluster J, w supports both trees a and r, 
i.e. w £ Portal (cr, t). 

We now proceed with the computational properties of 
Porto- r . As stated in the proof of Proposition 3, the inclusion 
test of a configuration for being in a hierarchical stratum can 


be computed in O J| 2 J time, from which one conclude 
that the inclusion test for being in Portal (cr, r) can also be 
computed in O time. If the given configuration is not 

a portal configuration, then the computation of Port CT T (33) 
requires cluster centroids of cr, which can be computed in 
linear, O (| J|), time as described in the proof of Proposition 
3. Given cluster centroids, one can compute Ctr (34) and Scl 
(35) in linear, O (| J|), time since the Napoleon transformation 
NT of an arbitrary triangle can be computed in constant, O (1), 
time [87], Finally, given the cluster centroids, each iteration 
of Mrg (37) can be computed in linear O (| J|) time; and so all 
iterations of Mrg can be performed in O Jj 2 ^ time since it 
may require at most | J| iterations. Thus, the result follows. ■ 


B. Proof of Proposition 3 

Proof Recall from (15) that for any singleton cluster I £ 
G(t) we have T>a(I) = ©(r). Hence, for any given x £ 
&(t) the base condition x £ Da (I) (Table IV .2) always holds 
at any singleton cluster I £ G(t). Moreover, the cardinality 
of any cluster passed as an argument in a recursive step of the 
evaluation must decrease relative to the calling cluster size 
(Table IV.7). Therefore, the recursion in Table IV terminates, 
in the worst case, after visiting all clusters of r only once. 

Since all vector fields (/^ (14), fn (19) and fg (24)) used 
in Table IV are well defined over the entirety of their domain 
© (t) with codomain (M. d ) ’ , the recursion in Table IV results 
in a true function, f Ty : ©(r) —» (M d ) , with well defined 
evaluation for each configuration x £ 6(t). 

We now assess the computational complexity of the recur¬ 
sion in Table IV. Centroids of clusters of r can be computed 
all at once in O (| J|) time using the post-order traversal of r 
and the following recursive relation of cluster centroids: for 
any disjoint A, B C J, 

c (x| A U B) = |A [^| B| c (x| A) + c (xjB). (67) 


Given cluster centroids, pij. T (x) (7) can be computed 
in constant, O (1), time for any i £ I and I £ G (r). 
Hence, since |C(r)| = 2 | J\ — 1 for any t £ 23T j and 
I = {fc | k £ K, K £ Ch (/, t)} for any nonsingleton cluster 
I £ G (t), we conclude: 

• The inclusion test for being in ©(r) (6) can be computed 
inO(|J| 2 ). 

• Given x £ ©(r), the inclusion test for being in T>h (I) 

(18) for any cluster I £ G (r) can be computed in 
O (| J|) time; and the recursion in Table IV requires such 
inclusion tests at most only once for all clusters of r 
which can be computed in O time. 

• The vector fields /a (14), fn (19) and fs (24) at any 
cluster I £ G(t) can be computed in 0(|J|) time; 
and, once again, the recursion in Table IV requires such 
computation at most at every cluster of r all of which 
can be performed in O (|J|j time. 


Finally, to conclude that f Ty is computable in 0 
time, we show that the inclusion test for being in Da (I) 
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(15) for all clusters I £ 6 (r) can be efficiently com¬ 
puted in O J| 2 ^j time as follows. Given cluster centroids, 

£-^(xfc — mjf(x| s_r-(x) (17) can be computed in constant, 
0(1), time for any k £ K, I\ £ C(r); and, likewise, 
||xj — Xj-11 2 (16) can be computed in constant 0(1) 
time for any given pair i ^ j £ J. Further, using (15) 
and hierarchical relations of clusters, observe the following 
recursive relation of Da (I): for any nonsingleton I £ G (r) 
and {I l ,Ir} = Ch(/,r), 

D A (I) = D a (I I ) n D A ( Ir ) D D A (I L , Ir ), (68) 

subject to the base condition D a (I) = 6(r) for any singleton 
cluster I £ 6 (r), where 

$>a (II , Ir) : = |x e 6(r) ||x f -x, || 2 > ( rt+rj ) 2 Mi £IlJ £Ir, 

(xfc - m K (x)) T sx (x) > 0, Mk £ K, I\ £ {I L , /«}}. (69) 

Note that, given x £ ©(r), the inclusion test for being 
in T>a(Il,Ir) for the children {Il,Ir} = Ch(7, t) of 
any nonsingleton cluster I £ G (t) can be computed in 
O (|/l| \Ir\ + \I L \ + \Ir\) time. 

Hence, given x £ ©(r), the inclusion test for being in 
Da (15) for any cluster I £ G (t) and all its descendants in 
Des (I, t) can be computed at once in O ^|/| 2 j time using 
the post-order traversal of the subtree of r rooted at / and the 
recursive formulation (68) of T>a (J)- This can be verified as 
follows. First, observe that the cluster set G (r) of r can be 
recursively defined as: 

• (Base Step) {j} £ G (r) for all j £ J. (70a) 

• (Recursion) If /, I~ T £ G (r)\{ J}, then Pr (I, r) £ G (r). 

(70b) 

Accordingly, we provide a proof by structural induction [92], 
For any I £ G (r): 

• (Base Case) If I is singleton, then the result simply holds 
since any singleton cluster I £ G (r) has no descendant in 
r and satisfies Da (I) = ©(r). 

• (Induction) Otherwise (|7j > 2), let {Il,Ir} = Ch(/,r). 

(Induction hypothesis) Suppose that the inclusion test for 
being in Da for any child K £ Ch (I, r) and all its 
descendant in Des (K , r) is computable in O ( \K\ 2 ). Then, 
by the recursion in (68), the inclusion test for being in 
Da for cluster I and all its descendants in Des (J, r) only 
requires the extra test for being in Da{Il,Ir) for the 
children {Il,Ir} = Ch (/, r) in addition to the inclusion 
test for every child K £ Ch (J, r) and its descendants in 
Des (K,t). Hence, the total computation cost for cluster 
I and its descendants in r is O + O (J/r| 2 ^ + 

O ( I Tl | | Ir | T | Ir \ T | Ir |) = O (| /1 2 ). 

Therefore, since C (r) = {J} U Des (J, r), given x £ ©(r) 
the inclusion test for being in Da {!) for all clusters I £ C (r) 
can be computed at once in O (|>^| 2 J time, and this completes 
the proof. ■ 


C. Proof of Proposition 4 

Proof To demonstrate how the recursion in Table VII recur¬ 
sively composes continuous vector fields, we shall recast Ja 
(14), fn (19), fs (24) and the recursion hg.b (Table VII) 
as follows: for any cluster I £ V T (3) (45) visited during 
recursive computation of hg } b, 


: 6(r)x (K d ) J -> 6(r)x(R d ) J 
(x,u) (x, f A (x, u, I)) 

f J H : ©(r)x (R d ) J ^ 6(r)x(R d ) J 
(x,u) H- (x, f H (x, u, /)) 

/|:©(r)x( WL d ) J ^r ©(r)x(R d ) J 
(x,u) (x,/ s (x,u,/)) 

: 6(T)x(R d ) J -> ©(r)x(R d ) J 
(x,u) ^ (x, h 3i b (x, u, I) j 


(71) 

(72) 

(73) 

(74) 


Note that, by definition, (x, u) is smooth in both x and 
u, and (x, u) and fg (x, u) are continuous and piecewise 
smooth functions of x and u since functions defined by the 
maximum of a finite collection of smooth functions are contin¬ 
uous and piecewise smooth, and the product of continuous and 
piecewise smooth functions are also continuous and piecewise 
smooth [93]. 

We now show that, for any I £ V T ( 3 ), hg b (x, u) is 
continuous and piecewise smooth in x and u. First, observe 
from Lemma 5 that the set V T ( 3 ) (45) can be recursively 
defined as 


• (Base Step) I £ V r {3) for all I £ 3- (75a) 

• (Recursion) If I,I~ T £V T (3)\{J}, then Pr (J,r)eV T (3). 

(75b) 


Accordingly, we provide a proof by structural induction [92], 
For any cluster I £ V T ( 3 ): 

• (Base Case) If I £ 3, then we have 


H,h ( x ’ u) 


//A ( X ’ U )’ if b/ = + 1 : 

\fs ( x - u )> if b/ = -1, 


(76) 


which is continuous and piecewise smooth in x and u. 

• (Induction) Else, we have |/| > 2 and so let {/^ ,Ir} = 
Ch (/, r). (Induction hypothesis) Suppose hg L h (x,y) and 
hg R b (x, y) are continuous and piecewise smooth. Then, one 
can verify from Table VII that 

hg,b Or U) = (/A O hg R b o hg^hj (x, u) . (77) 


Hence, hg h is a composition of continuous and piecewise 
smooth functions, hence it must remain so as well [93], 
Thus, the result follows since (x, b (x)) = hg b (x, 0). ■ 


D. Proof of Proposition 5 

Proof Since the recursion in Table VIII uses only clusters of 
r and guarantees, in Table VIII.4), Table VIII. 7) and Table 
VIII. 13), that the dimension of b is equal to the cardinality of 
3, the output (3, b) = p (x) associated with any configuration 
x £ &(t) always satisfies that 3 C C (r) and b £ {—1, +1} 3 . 
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To prove that 3 is a partition of J, we shall show that, 
for any x G ©(r) and 7 G C (t), (3, b) = p(x, 7) yields 
a partition J of 7. Based on the recursive definition (70) of 
C (t), we now provide a proof by structural induction. For 
any x G 6(r) and 7 G C (r) let (J, b) = p (x, 7), then: 

• (Base Case) If 7 is singleton, then Da (7) = ©(r) and 
the base condition in Table VIII.2) holds. Hence, we have 
3 = {/}, the trivial partition of 7, and the result follows. 

• (Induction) Otherwise (|/| > 2), we have two possibilities. 

-If 7 satisfies any base condition in Table VIII.2) and in 
Table VIII.5), i.e. x G Da (J) U (6(r) \ X> H (/)), then 
we have 3 = {/} and the result directly follows. 

- Else(the recursion condition in Table VIII. 8)-14) holds), 
since |/| > 2, let {Il,Ir} = Ch (I, r) and (CJl, bt) = 
p(x, I L ) and (3 R ,b R ) = p(x,I R ). (Induction Hy¬ 
pothesis) Suppose that 3 £ and 3# are partitions of I R 
and I R , respectively. Then, since 3 = Jl U J r (Table 
VIII. 12)) and Ch (7, t) is a bipartition of 7, we observe 
that 3 is a partition of 7. 

Hence, since (3,b) = p (x) = p(x, J), the recursion in 
Table VIII terminates with a partition 3 of J. Thus, since the 
policy selection algorithm is deterministic, p is a well-defined 
function from 6(r) to SCPj (r) (42). 

Finally, we shall show that (3, b) = p (x) is the index 
of a local control policy whose domain CD (3, b) contains 
x, i.e. x G (Bop)(x). Using the base conditions in Table 
VIII.2)-7) one can verify that for any 7 G 3, if bj = +1, 
then x G Da (7); and if b/ = —1, then x G ©(r) \ 
(Da (7) U (/)) C ©(t). Hence, the base conditions 
guarantee that x G CD^ (7, b/) (44) for any 7 G 3- Observe that 
during the recursive computation of p in Table VIII to reach 
any cluster 7 G 3 satisfying a base condition every ancestor 
77 G Anc (7, t) of 7 must have been recursively visited. A 
recursion (Table VIII.8)-14)) at any ancestor K G Anc(7,r) 
of 7 in t implies that x G D R (77) \ D a (77) C T>h (77). 
Thus, by definition (43), we have x G D (3, b) and the result 
follows. ■ 


£C Proof of Proposition 6 

Proof For any given x G ©(r), the recursions in Table IV 
and Table VIII traverse the same clusters of r in the same 
order since both recursions have identical base and recursion 
conditions. 

Now observe that the tree traversal pattern used by the 
recursion in Table VII is fixed for a given policy index 
(3, b) G SCPj (t): a base condition is satisfied at any cluster 
7 G 3, and to reach such cluster 7 all its ancestors Anc (7, r) 
must have been recursively visited starting from the root J. 
Recall from the proof of Proposition 5 that (3, b ) = p (x) 
yields a partition 3 of J such that a base condition in Table 
VIII holds for every block 7 G 3 and all its ancestors in 
Anc (7, t) are recursively visited. Hence, if the policy index 
is selected as (3, b) = p(x), the recursion in Table VII 
computing hg b (x) always follows the tree traversal pattern 
used by the recursion in Table VIII computing p (x). 


Thus, for a given configuration, all recursions in Table 
IV, Table VII and Table VIII share a common tree traversal 
strategy. 

Let x G ©(t) and (3, b) = p(x), and observe from Table 
VIII that for any 7 G 3, if bj = +1, then x G Da (7); 
and if b/ = —1, then x G ©(r) \ (Da (7) U D R (7)); and 
x G D h (77) \ Da (77) for all K G Anc (7, r). Using this 
relation between policy indices and domains, one can conclude 
that the recursions in Table IV and Table VII use the same 
vector fields for the identical base and recursive steps. Thus, 
the result follows. ■ 


F. Proof of Proposition 7 

Proof According to Definition 5, any pair ((3, b), (3', b')) 
of policy indices in £yg satisfies at least one of Lemmas 
10 - 12. Hence, hg ,b prepares hg>.w in finite time, and 
priority^', b') > priority(3, b). Thus, ((3, b), (3', b')) 
is also an edge of the prepares graph CPS- 
Moreover, for any (3, b) ({7} , +1), there always exists 
a policy index J3',b') (3, b) such that ((3, b), (3', b')) 

is an edge of CPS- This can be observed as follows. Since 
3 is compatible with r, i.e. 3 C 6 (r), if |3| > 1, then there 
exists a nonsingleton cluster 7 G C (r) such that Ch (7, r) C 3 
(Lemma 5). Hence, at least one of the following always holds: 

(a) There exists a cluster 7 G 3 with bj = —1. 

(b) There exists a cluster 7 G C (r) such that Ch (7, r) C 3 
and b R = +1 for all 77 G Ch(7,r). 

And (3',b') can be selected accordingly to satisfy one of the 
connectivity conditions of CPS (Definition 5.(i)-(iii)). 

Since every policy index (3,b) ({J},+1) has an 
adjacent policy index (3',b') in CPS and priority(3' > b') > 
priority(3, b), CPS has no cycle and all of its nodes con¬ 
nected to the goal policy index ({J},+1) through directed 
paths along which priority is strictly increasing. Note that 
the goal policy index has the highest priority value which 
is | J\~ (49). Further, since priority (48) is integer valued 
function whose range (49) is [— |J| ,|Jf], the length of a 
directed path in CPS is bounded above by O J| 2 ^) hops, and 
the result follows. ■ 


G. Proof of Proposition 8 

Proof If there is only one local controller whose domain 
contains x, then the result follows from Proposition 5. 

Otherwise, we shall provide a proof by contradiction. Let 
(3, b) = p (x), and (3', b') be the index of a local controller 
whose domain D(3', b') (43) contains x, and (3', b') (3, b). 

Suppose that the local controller hg>^ has the maximum 
priority among all local controllers whose domains contain 
x. We shall show below that there always exists another local 
controller whose domain contains x and it has a higher priority 
than priority(3 / ,b'), which is a contradiction. 

It follows from Lemma 6 that at least one of the followings 
always holds: 

• Case 1 (3 Partially Refines 3')- There exists a cluster 77' G 
3' with a nontrivial partition 37' (i.e. |37'| > 2) such that 
37' C 3- Since x G D (3, b) and all the elements of 37' 



ESE TECHNICAL REPORT — JULY 8, 2015 


22 


are descendants of K' in r, the recursive tree traversal in 
Table VIII requires that x £ T> h(K') \ T>a{K')- Hence, 
b 'k 1 = ~ 

Since (a + b) > a 2 +b 2 for any a, b £ R + , one can observe 
that replacing K ' of 3' with the elements of X' and updating 
b' with the associated binary values from b yields the index 
(3", b") of another local controller, 


3" = X'U3'\{K'}, 
b" = (b'/W' s.t. b? 
at a strictly higher priority. 


/b/, if I £ X', 

\b', if l£d'\{K'}, 


(78) 

(79) 


priority(3",b") = priority(3',b') + |A"| 2 + ^ b//|7'| 2 , 

/'ex' 

> 0 


> priority^', b 4 ). (80) 


Note that we still have x £ D(3", b") since x £ D(3,b) fl 
D(3',b'). 

• Case 2 (3' Partially Refines 3)' There exists a cluster K £ 
3 with a nontrivial partition X (i.e. |3C| > 2) such that 
X C 3' ■ Since K £ 3 , one of the base conditions in Table 
VIII at cluster K holds, and so we have either x £ T>a (K) 
or x ^ T>h ( K ). Further, since x £ T)(3',b') and K is an 
ancestor of all the elements of X in r, we have x £ T>h (7\). 
Therefore, x £ 1>a ( K ) and \>k = +1. 

Once again, since (a + b) 2 > a 2 + b 2 for any a, b £ R+ and 
x € 2)(3,b) fl D(3 , ,b'), one can verify that the following 
local policy index 


3" = {K} U a' \ X, (81) 

b = ( b i)ie3" s - 1 - bj = | b /_ , if I £ 3' \ X, (82) 

has a strictly higher priority, 

priority(3",b") = priority^', b') + | jC| 2 -y^b/|J| 2 , 

/ear 


> o 

> priority(3', b'), (83) 


and its domain contains x, i.e. x £ T>(3", b"). 

• Case 3 (Identical Resolution): 3' = 3 and b' / b. Since 
3' = 3. one can maximize priority^ 4 , b') (48) by 
maximizing the binary vector b', which is achieved by 
setting b'j = +1 for any I £ 3' whenever x £ T>a(I)- 
The base conditions in Table VIII guarantee such an optimal 
selection of IT. However, since b 4 f b, we have 

priority(3, b) > priority(3\ b 4 ), (84) 

which completes the proof. ■ 


H. Proof of Proposition 10 

Proof The continuity and piecewise smoothness of hg,b 
(Proposition 4) implies its locally Lipschitz continuity in &(t) 
[89]; and the existence of at least one trajectory of hgy, starting 
at x follows from its continuity. 


Let x* denote a trajectory of hg b starting at any x° £ 
T> (3- b) for all t > 0. We have from Proposition 9 that x 4 
remains in D (3,b) for all t > 0. Further, by Lemma 3, the 
centroidal trajectory c(x*|J) is guaranteed to lie on the line 
segment joining c (x°|J) and c(y|J); and, by Lemma 4, the 
centroidal configuration radius r (x*| J) (12) is bounded above 
by a certain finite value, f?(x°,y), depending only on the 
initial and desired configurations, x° and y, respectively. Thus, 
all trajectories of /ig b stay in a compact subset W of D (3, b) 
and the compact set defined by the Minkowski sum of the 
line segment joining c (x°| j) and c (y| J) and the closed ball 
centered at the origin with radius of R (x°, y). 

Given that all trajectories of h.g,b starting at any x £ 
T> (3, b) lie in a compact subset W of D (3, b), the uniqueness 
of its flow follows from the Lipschitz continuity of hg .b in W 
since a locally Lipschitz function on ©(t) is Lipschitz on 
every compact subset of ©(r), also refer to Theorem 3.3 in 
[82], Moreover, this unique flow is continuous and piecewise 
smooth since it is the integral of the continuous and piecewise 
smooth vector field h.g,b [94], which completes the proof. ■ 

I. Proof of Proposition 9 

Before proceeding with the proof of Proposition 9, we find 
it useful to emphasize some critical properties of a trajectory 
x 4 of /ig.b (x) starting at any x° £ D (3, b). 

Lemma 7 A trajectory x 4 of /y. g v> (x) (Table VII) starting 
at any initial configuration x° £ CD(3,b) (43) satisfies the 
following properties for any I £ 3 with b/ = +1 and t > 0, 

(i) - Xj 4 || 2 > (n + G) 2 , fi^j£l, 

(ii) Lr^(x t k — hir:( x 4 )) T sjf (x 4 ) >0, Vk£K, K£Des (I,t) , 
(Hi) 9k ,k (x 4 ) >0, Vfc £ I\, K £ Des (I, r), 

(iv) ||x| - x 4 1|" > (n + Tj) 2 , Vif=j£l. 

Proof See Appendix II-M. ■ 

Lemma 8 A trajectory x 4 of hyyy, (x) (Table VII) starting 
at any initial configuration x° £ CD(3,b) (43) satisfies the 
following properties for any I £ 3 with b/ = — 1 and t > 0, 

(i) J?fe,ir(x 4 ) >0, Vfc S I\, K £ Des (/, r), 

(ii) ||x 4 - x 4 1|" > (n + rj) 2 , Viylj £ I. 

Proof See Appendix II-N. ■ 

Lemma 9 Let V T (3) (45) be the set of clusters visited during 
the recursive computation of hg,h (x) in Table VII. 

Then a trajectory x 4 o/Zig^ (x) starting at any initial con¬ 
figuration x° £ T) (3, b) (43) satisfies the following properties 
for any I £ V T (3) \ 3 and t > 0, 

(i) 9 k , K (x 4 ) > r k + a, Vk £ If I\ £ Ch (7, r), 

( ii) 11 x 4 - x 4 11 2 > (u+rj ) 2 Vi S K, j £ I\K, K £ Ch (I, r). 

Proof See Appendix II-O. ■ 

Accordingly, we conclude the positive invariance of the 
domain T> (3, b) of hg,b as follows: 

Proof of Proposition 9 By Lemma 7.(iii)-(iv) and Lemmas 
8-9.(i)-(ii), a trajectory x 4 of hg b starting at any x° £ D (3, b) 
is guaranteed to remain in ©(r) for all future time. Given 
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x 4 £ 6 (t) for all t > 0, Lemma 7.(i)-(ii) imply x 4 £ Da ( I ) 
for any / £ 3 with b/ = +1; and Lemma 9.(iii) implies 
x 4 £ Dh (K) for every ancestor K £ Anc (I, r) of any I £ 
3. Thus, by definition (43), we have x 4 £ D (3, b) for all 
t > 0. ■ 


(b) There exist a cluster / £ 6 (r) such that Ch (/, r) C 3 
and = +1 for all K £ Ch (/, t). Accordingly, the 
results follows from Lemma 12 and this completes the 
proof. ■ 


J. Proof of Proposition 11 

Here we first establish finite-time prepares relations between 
pairs of local policies whose indices are related to each other 
in a certain way as specified in Definition 5; and then we 
continue with the proof of Proposition 11 . 

Lemma 10 (The Case of Definition 5.(i)) Let 3 £ Tj (r) be a 
partition of J andh,W £ {—l,+l} a . If hi = b \ for all I £ 3 
but a singleton cluster D £ 3 where b= —1 cnitf b 4 D = +1, 
then the domains (43) of local control policies hg,b and kg, b' 
are identical, i.e. 

T>(8, b') = D (3, b), (85) 

and their priorities (48) satisfy 

priority (3, b') = priority (3, b) + 2. (86) 

Proof See Appendix II-P. ■ 

Lemma 11 (The Case of Definition 5.(ii)) Let 3 £ Vj (t) be 
a partition of J and b £ {—1, +1}^ such that b/ = —1 for a 
nonsingleton cluster I £ 3; and let 3' = 3 \ {/} U Ch (/, r) 
b' £ {—1,+1} 3 with b' K = —1 for all I\ £ Ch (/, r) 
and b' D = bn for all D £ 3 \ {/}. 

Then all trajectories of the local control policy hg t b starting 
in its domain D (3, b) reach infinite time the domain D(3', b') 
of the local controller /ig\b' which has a higher priority 
(48) than hg t b does, i.e. 

priority(3', b') > priority (3, b). (87) 

Proof See Appendix II-Q. ■ 

Lemma 12 (The Case of Definition 5,(iii)) Let 3 £ Dj (r) be 
a partition of J and b £ {—1,+1}^ such that Ch (/, t) C 3 
for a nonsingleton cluster I £ 6 (r) and bx = +1 for all I\ £ 
Ch (I, t); and let 3' = 3\Ch (I, r)U{/} andh’ £ {—1, +1} 9 
with bj = +1 and b' D = bp for all D £ 3 \ Cli (I, r). 

Then the local control policy hg ,b steers (almost) all con¬ 
figurations in its domain D (3,b) in finite time to the domain 
D(3', b') of the local controller hg'w which has a higher 
priority (48) than hg,b does, i.e. 

priority(3 / , b') > priority (3, b). (88) 

Proof See Appendix II-R. ■ 

Proof of Proposition 11 Since 3 is a partition of J compat¬ 
ible with r, i.e. 3 C C (r), observe that if |3| > 1, then there 
exists a cluster I £ C (r) such that Ch (/, r) C 3 (Lemma 
5). Hence, since (3, b) ({J}, +1), at least one of the 
followings always holds: 

(a) There exists / £ 3 such that b/ = —1. If |/| = 1, then 
we have the result by Lemma 10; otherwise (|/| > 1), 
the results follows from Lemma 11 . 


K. Proof of Lemma 3 

Proof For any cluster I £ 3 the recursion in Table VII 
employs a vector field satisfying the associated base condition, 
and then recursively constructs an additive repulsion field at 
every ancestor Anc (/, r) of I, which can be explicitly written 
as follows: for any i £ / and I £ 3, 

• if bj = +1, then we have 


u i = 


f A {x.,0J)i + ^2 2a fi (x,v fl ) 

i<rGAnc(/,r)U{/}\{ J} 
R—Pr(K,r) 


\ K T \ Sjf(x) 

\R\ IMx)|| 


,(89) 


• else (b/ = — 1), 


Ui = 


/s(x,0,/)*+ Y] 2a fl (x,v fl ) 

K’GAnc(/,r)U{/}\{J} 

R=Pr(K,r) 


\K T \ Sif(x) 

1*1 IMx)|| 


, (90) 


for some £ (K d ) J associated with cluster R £ Anc (I, r). 
Now, using (14) and (24), one can verify that for any / £ 3, 


c(u\I) = -c{x-y\I) + Y 2 a R (x,v R ) 

K’GAiic(/,t)U{/}\{ J} 
R=Pt(K,t ) 


\K~ 


sjr(x) 


|A| ||sk(x) 


(91) 


which can be generalized to other clusters in V T (3) \ 3- 
That is to say, we now show that for any I £ V T (3) 
the centroidal dynamics c (u|7) satisfies (91). Based on the 
recursive definition (75) of V T (3), we provide a proof by 
structural induction. For any I £ V T (3), 

• (Base Case) If I £ 3, then the result is shown above in (91). 

• (Induction) Otherwise, |/| > 2 and let {Il,Ir} = Ch (/, r). 
(Induction hypothesis) Suppose that c(u|/l) and c (u|/r) 
satisfy (91). Then using 

C ( U I J ) = ]7[' c ( u I J l ) + ^ c ( u l 7 «)> (92 ) 

one can obtain (91) for cluster / as well. 


Observe that for the root cluster J the equation (91) 
simplifies and yields (64). Further, using (91), we obtain (63) 
for any I £ V T (3)\{</} with parent P = Pr (J, r) as follows: 


c(u|J) = -c(x-y|/) + 2ap (x,vp) 


\I~ T \ S/ (x) 


1*1 l|s/(x) 


y: 2a K (x,v fl ) 


|7v “ 


sjf (x) 


ArGAnc(/,r)\{ J} 
i?=Pr(AT,T) 


1*1 l|Sif(x) 


,(93) 


= -c (x—y|/) + 2ap (x, v P ) 


=c(u|P)+c(x—y|P) 

\I- T \ S/(X) 


*1 l|s/(x)|| 

+ c(u|P) + c(x-y|P), (94) 


which completes the proof. 
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L. Proof of Lemma 4 

Proof Since the domain T> (3, b) of hg .b is positive invariant 
(Proposition 9), the existence of x* for t > 0 simply follows 
from the continuity of hg b (Proposition 4). We now show 
that for any I £ V T (3) (45) visited during the recursive 
computation of hg ,b in Table VII the centroidal radius r (x 4 | I) 
is bounded above by a certain value, Ri (x°,y), depending 
only on x° and y. 

Based on the recursive definition of V T (3) in (75), we now 
provide a proof of the result by structural induction. For any 

i g v r (a), 

• (Base Case 1) If / G 3 and |/| = 1, then the result simply 
follows since r (x 4 |/) = r 7 ; for all t > 0, where I = {i}. 

• (Base Case 2) If I £ 3, |/| > 2 and b/ = +1, then, using 
Table VII, one can verify that for any i € I 

±i = hg h (x) { = f A (x, u, I) i + v z , (95a) 
= - (xj -yf) +vj, (95b) 

for some u G (R d ) J and v/ G where vj represents the 
accumulated rigid translation due to all ancestors of I in r. 

Accordingly, we obtain for any i £ I that 
^||x i -c(x|/)|| 2 = -2||x i -c(x|/)|| 2 

+ (xi —c (x|/)) T (y i -c (y|/)),(96) 
from which one can conclude that 

|| x ‘- c ( xt | / )||< m ax(||x°-c(x°|/)||, ||yi-c(y|/)||).(97) 

Thus, by definition, it follows that the centroidal radius 
r (x 4 | I) is bounded above as 

r (x 4 |/) < Rr (x°,y) = max (r (x°|J) ,r(y|7)) . (98) 

• (Base Case 3) If I £ 3 , |/| > 2 and b/ = —1, then, using 
Table VII, one can verify that for any k £ K and I\ £ 
Ch (J,r) 

Xfc = hg, b (x) fc = f s (x,u, I) k + V/ , (99a) 

= -c (x—y| J) + 2/3/ (x) i| S ^! X ||| +W, (99b) 

\I\ ||sif(x)|| 

for some u £ (R d ) J and vj £ R d . 

Accordingly, we obtain for any I\ £ Ch (I, r) that 

j t r (x|iT) = 0, (100) 

^||s if (x)|| 2 = 2/3/(x). (101) 

Observe from (25) that 

||sk(x)|| > 2 W + max r(x|D)]=4- min ( r / d,D (x) -r d ) > /3, 

V D6CM7,t) ) d£D 

V 7 £>eCh(/,r) 

=4- fa (x) = 0, (102) 

Thus, since r (x 4 |/C) = r (x°|A') for all t > 0, it follows 
that 

11sir (x 4 ) || < max^||sx (x°) ||, 2^/3+^_max r (x°|.D)jj, (103) 


and, since r (x| I) < max^eCh(i» l|sir(x)|| +r (x, K) for 
any x £ 6(r), we have 

r (x*|TVT) < Ri (x°,y) , (104) 

where 

i?/(x°, y)= max max( lls# (x°)||, 2(/3+r (x° 
v ' JtreCh(/,r) V v V v 

+ max r (x°|A'), (105) 

KeCh(I,r) y ' 

• (Induction) Otherwise, |/| > 2 and suppose that r (x 4 | K) < 
Rk (x°,y) for all K £ Ch(J, r). Then, using Lemma 3, 
one can obtain for any K £ Ch (/, r) that 

4;ll s x( x )|| 2 = -2 ||sk(x)|| 2 +2sk(x) t s^(y)+2aj(x, vj), (106) 
at 

for some vj £l d . Now observe from (20) that 



l|sx(x)|| > 2 


\B+ max r (x|D) 

y D£Ch(i, T ) j 


=+ min (rjd,D(x-)-rd) > P, 
d£D 

DeCh ( I , r ) 

=+ oil (x, V/) = 0, (107) 


Hence, using (106) and (107), one can conclude that 

11sat (x 4 )|| < max || s K (x°)||, ||sx (y)||, 2 (ft + max R D (x°, y)jj 

(108) 


and since r (x|/) < maxx e ch(/,r) ||sk(x)|| 2 +r (x, K) for 
any x £ 6(t), we have 

f (x 4 |7) < Ri (x°,y) , (109) 


where 

Rl ( X °’IdcMi (l Sk ( x °)ll ’ ll s x(y)l|, 2 [f+Rn (x°, y)| 

+ max Kk (x°, y) . (110) 

JfeCh(/,r) v ' 


Thus, the result follows with R (x°,y) = i?j(x°,y). ■ 


M. Proof of Lemma 7 

Proof By definition of T> (3, b) (43), x° £ T>a (I) for any 
I £ 3 with b i = +1, and one can verify using Table VII that 
for any i £ I and I £ 3 with b/ = +1 


xj = hg, b (x) 4 = f A (x,u,/) i + vj, (111a) 

= - (x»-yj)+vj, (111b) 

for some u £ (K. d ) J and v 7 £ R d , where v 7 represents the 
accumulated rigid translation due to ancestors of I in r. 

Accordingly, Lemma 7.(i)-(iv) can be shown as follows: 

(i) Using (16) and (1 1 1), one can verify that for any i f j £ I 

2 11 x? Xj 11 2 = ||xi — x 7 1| 2 + ||y* — y*l| 2 ,(H2) 

> (ri+rj) 2 , since yG6(r) 

> H x * _ Xl H 2 + ( r * + r i) 2 ,(H3) 


and so for any t > 0 




> e- 4 C^i||x°-x0|| 2 +(l-e- 4 )(r i +r J ) 2 

> (T’i+rj) 2 , since x°E!Da(-0 

> ( ri+rj) 2 . 


(114) 
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(ii) Similarly, using (17) and (111), we obtain for any k £ 

K,K £ Des (7, r) 

^£y-(xfc-SA'(x)) T SA'(x) = -£^-(xfc-SA'(x)j r S A (x) 

+ Vk,K(y) ||s*r(y)||, (115) 

v -v-' 

>0, since y £©(t) 

from which we conclude for any t > 0 


Accordingly, using (122), we will show the results as 
follows: 

(i) The preservation of local cluster structure can be observed 
in two steps. First, since x, x :i 0 for any i ^ j £ K 
and K £ Ch (/, r), we have r/d,D (x‘) = r]d,D (x°) > 0 for 
all t > 0 and d £ D, D £ Des ( K , r) and AT £ Ch ( K, r). 
Second, using (23) and (25), we obtain that for any k £ K, 
K £ Ch (7, r) 


£^(xfc-SA(x‘)) T SA(x t ) >e * (x° — sa (x°)) T sa (x°), 

--v- 

> 0, since x°GDa(-0 


>o. 


(116) 


(iii) Now observe from (17), (23) and (111) that for any k £ K 
and K £ Des (I, r) 

d , , , Ui , Sjc(x) T SA(y) N \ 

—r]k,K{yi) = -?7fe,A(x) in--— 1 


I|sjc(x)|| J 
- m K (x) ) T s K (x) 
||sa(x)|| 


• (117) 


> 0 by Lemma V. (ii) 


As a result, since Jr? 7 fc,A(x) > 0 whenever rjk,K (x) = 
0, we have the invariance of local cluster structure, i.e. 
> 0 for all f > 0. 

(iv) The relative displacement of any pair of agents, i ^ j £ I, 
satisfies 


x, -ij = - (xi - x j) + (y» - yj) ■ (118) 

whose solution for t > 0 is explicitly given by 

x* - x* = e -t (x° - x°) + (1 - e _t ) (y* - y,). (119) 

Hence, since x° £ T>a (7) and y £ @(r), one can verify 
the intra-cluster collision avoidance as follows: 



> (n+r-j ) 2 > (ri+rj ) 2 


+ e * (l-e *) 11x° — X°11 2 , (120) 

s, ^ 

> 2 (n+rj) 2 

>(ri+fj) 2 , (121) 

and this completes the proof ■ 

N. Proof of Lemma 8 

Proof For any singleton I £ 3 the results simply follow since 
a singleton cluster contains no pair of indices and has an empty 
set of descendants. Otherwise, for any nonsingleton I £ 3 with 
b/ = — 1, one can obtain from Table VII that for any k £ K 
and I\ £ Ch (/, r) 

ifc = h 3>h (x) fc = f s (x, u, I) k + v/, (122a) 

= -c(x— y|7) + 2/3j(x) ^ +vj, (122b) 

\I\ 11 sat (x) (j 

for some u £ (M. d ) 1 and v/ £ K" 1 , where vj models the 
overall rigid translation due to ancestors of I in r. 


^7fc,jr(x)=/3/ (x) > - (r) k}K (x)-rk~P) , (123) 

where /? > 0. Hence, a (x) > 0 whenever r/fc, a (x) = 
0, and so r) k a (x 4 ) > 0 for any t > 0. 

(ii) Likewise, we conclude the intra-cluster collision avoidance 
between individuals in I in two steps. First, we have for 
any i j £ K, K £ Ch (J, r) 

x, - x, = 0, (124) 


guaranteeing that for all i > 0 


X--X* 2 = x°-x° 2 > {ri + Tjf . 

(125) 

Second, for any i £ K, j £ I \ K and K £ Ch (7, 
have 

t) we 

00 , ^ Sa(x) 
x, x,=2ft(x) ||sjfWr 

(126) 

yielding 


M ii* x >“ = 2 *w (x ‘ Xj) ii«(x)ir 

>o x -v-' 

=W*,k(x)+Vj,i\k(x)> 0 

(127) 

>0, 

(128) 

and so for t > 0 


X* -X* 2 > X° -x° 2 > (n + r 3 f . 

(129) 


O. Proof of Lemma 9 

Proof By definition of CD (3, b) (43), for any I £ V T (3) \ 3 
we have x° £ T>h (7) (18) and one can verify from Table VII 
that for any k £ K and I\ £ Ch (/, r) 


xjc = h 3: b (x) fc = f H (x, u, I) k + v/, (130a) 

, x \K~ T \ sa(x) 

= Ufc + 2ai (x, u) 1 -- —77 +v/, (130b) 

|7| ||sa(x)|| 

for some u £ (K. d ) 7 and v/ G R d . Here, vj represents the 
total rigid translation due ancestors of I in r. 

With these observations in place, we now achieve claimed 
results as follows: 

(i) The maintenance of cluster separation (Lemma 9.(i)) can 
be observed, using (23) and (130), as follows: for any k £ 
K and K £ Ch (7, r) 


^7fc,Jf(x) = L-^r)k,K(x) +ai (x, u), 


(131) 
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and, since x° £ T>h (I) and ct/(x, u) > —L^rjk,K{^) 
whenever %,jc(x) = r k + a, we have r/fc^ (x*) > r k + a 
for all t > 0. 

(ii) The inter-cluster collision avoidance (Lemma 9.(ii)) di¬ 
rectly follows from the maintenance of certain cluster 
separation (Lemma 9.(i)) since 

Vk,K{ x 4 ) > r k , ||x| —x*-1| 2 > (n+Vj) 2 , 

VkeK, => \/ieK,jel\K, (132) 
K sCh (/, r), K £ Ch (I, r). 


P. Proof of Lemma 10 

Proof Since D 4 (I) = @(t) for any singleton cluster I £ 
C (t), we have from (44) that Db (/, —1) = Db (/,+1) = 
6(t) for any singleton cluster I £ C (t). Hence, by definition 
(43), the first part of the result holds. 

Likewise, using (48), one can observe the second part of 
the result because the binary vectors b and b only differ at a 
singleton cluster D £ 3 where b r_> = — 1 and b^, = +1. ■ 


and 

D(3',b') = 2)(3,b)n !„(/), (138) 

D T) (3, b) n T>h (I). (139) 

Thus, since (/) C T>h ( I ) and its domain T> (3, b) is 
positively invariant (Proposition 9), hg,b prepares h-j'.h' in 
finite time, and the result follows. ■ 

R. Proof of Lemma 12 

Proof Since Ch (/, r) C 3 and \>k = +1 for any K £ 
Ch (/, r), every child K £ Ch(/,r) of I in r satisfies the 
base condition in Table VII.2)-4) whereas cluster I satisfies the 
recursion conditions in Table VII.9)-12). Hence, using Table 
VII, one can verify that for any k £ K and I\ £ Ch (/, r) 

Xfc = hg, b (x)fc = Uh ° /a) (x, u, I) k + V/, (140a) 

= ~ (xfc—y fc )+2a/(x,u) ^ ^ ||sk(x)|| +Vj ’ (140b) 


Q. Proof of Lemma 11 

Proof For any nonsingleton I £ 3 with b j = — 1, one can 
verify from Table VII that for any k £ K and K £ Ch (J, r) 


for some u £ (R d ) ’ 7 and v/ £ 

We now show in three steps that hg ,b asymptotically steers 
(almost) all configuration in its domain T> (3, b) to 


Xfc = hg,b(x) k = fs (x,u,7), (133a) 

= -c (x— y\I) + 2ftj (x) ^ 1 || SJC | X |, +vj, (133b) 

Pi l|Sif(x)|| 

for some u £ (K d ) J and vj £ R d . 

Accordingly, using (23) and (25), we obtain that 

( x ) ^ -RKk (x) + r k + ft. (134) 
at 

Hence, a trajectory x* of hg_ b starting at any x° £ T> (3, b) 
satisfies 

> e.~ t ri kt K (x°) + (l - e _t ) (r k +/3 ), (135) 

for all t > 0. Thus, since ft > a > 0 and jjjr]k.K (x) > 0 
whenever r/h.K (x) < r k + ft, using LaSalle’s Invariance 
Principle [95], one can conclude that the local policy hg.b 
asymptotically steers all the configurations in its domain 
D (3, b) to a subset (/) of the interior T>h ( I ) of D# (J) 
(18), 

<(I): = {xe S(r) |%,x(x) > r*-h8, Vfc G A', AT £ Ch (I, r)}, 

C T>h (/) • (136) 

In particular, since ft > a, the system in (133) starting at any 
configuration in D (3, y) enters T>h (I) in finite time. 

Now observe from (43) and (48) that 

priority (S', b') = priority (3, b) + |/| 2 - ^ |D| 2 , 

D£Ch(J,r) 


9 (!) : = |xg6(t) 


Sff(x) 


sg(y) 


I|S^(X)|| — iis^tF)iT’ ll s ^( x )ll > ll s ^(y)IU 
Xfc-C (x| K) =yfc-c (y| K), 

V k£K,K£Ch(I,T)Y (141) 


which is a subset of T>a (II > Ir) (69) associated with children 
clusters {Il,Ir} = Ch(7, t) because for any x£ 9 (/) and 
i£K, j £l \K and K£ Ch (I, t) 

Lrf±\\x i -x j \\ 2 = (x i -x i ) T (y i -y j ), (142) 

= ( x i—Xj — Sk (x)+ s k (y )) T (y j — y 3 -) 

'-V-" 

=y*-yj 

+ (sjf (x) —s_ff (y)) T (y» —yj), (143) 

=e s/c(y) for some e>0 

S ---V-" 

>0 since yG6(r) 

> l|y» - yjf > (n + t ?) 2 , (144) 

and for any x£ 9 (I) and k £ K and K £ Ch (I, r) 

T T 

£-^(xfc —niftr(x)) s K (x) = (yfc-mjf(y)) s K (x) 

v ^ 

>0 since xGS (I) and yE<5 (t) 

+ (xfc-mif(x)) T s K (y), (145) 

S. y ✓ 

>0 since x£S(/) 

> 0. (146) 


> priority (3, b) , 


>o 


Likewise, one can observe that 9 (I)n'DH (I) is a subset of 

(I 3 7 ) the j nter i or 0 f % A i.e. g (I)nT) H (/)c$a ( Il,Ir )• 
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First, using (140), we obtain that for any K E Ch (/, r) 

d Sjf (x) T s/f(y) = |[sjc (y) || s A -(x) T Sjf (y) 

dt ||sat(x)|| ||sjf(y)|| ||sif(x)|| || Si f(x)|| 3 || Si f(y)|| ’ 

= IISir (y) II L s g (x) T Sjf(y) \ 

||sir(x)|| ( ||sjr(x)|| ||s/f(y)||y ’ 

> 0 (147) 

where the equality only holds if ||^[ x ]|| = =*= ||s^(yj|| • Thus > 
hg } b asymptotically aligns the separating hyperplane normals 
of complementary clusters Ch (/, r) of (almost) any config¬ 
uration in D(3,h) with the desired ones. Note that the set 
of configurations x <E D (0,b) with = ~ [yjn has 

measure zero and are saddle points. 

Next, let x € D (3. b) with M Sf 4 x ||. = |, SK l y s|| for all K £ 
Ch(7,r). Then, using (140), observe that 

j t l|sif(x)|| 2 = -2 11s*r(x)|| 2 + 2s k (x-) T s k (y) 

+ 4a/(x, u) 11 Sir (x) 11, (148) 

> o 

> -2 ||sir(x)|| 2 + 2sir(x) T Sir(y), (149) 
= -2 ||sir(x)|| (||sa'(x)|| —||sir(y)||). (150) 

Hence, ^ ||sa(x )|| 2 > 0 whenever ||sa(x)|| < ||sir (y) || 2 - 
Thus, the stable configurations of hgg, also satisfies 
||sa(x)|| > ||sjr(y)||. 

Finally, we have from (140) that for any k £ K, K £ 
Ch (AC t) 

^ (x fe -c(x|A'))=- (xfe —c (x| AT)) + (yfc —c (y|AT)), (151) 

and so a trajectory x* of hgg, starting at any x° £ D (3, b) 
satisfies 

Xfc-c (x*|A) =e _t (x°-c (x°|A)) + (l-e _t ) (y fc -c (y|A)).(152) 

for all t > 0. Hence, the centroidal displacements, xj. — 
c(x 4 |A'), of any configuration x £ D(3, b) asymptotically 
matches the centroidal displacement, y/. — c (y| A'), of the 
desired configuration y. 

Thus, it follows from LaSalle’s Invariance Principle [95] 
that (almost) all configurations in the domain D ( 3 , b) of hgg, 
asymptotically reach S (/). 

Now observe from (43) and (48) that 

priority (S', b 4 ) = priority (3, b) + |/| 2 — ^ |A >| 2 

DeCh(/,r) 

"-V-' 

> 0 

> priority (3, b), (153) 

and 

D(3', b') d D (3, b) n D A (I L , I R ), (154a) 

DD(3,b)nS(/), (154b) 

which follows from that S (J) C Da ( Il,Ir ) and Da (I) = 

D a (-Cl) n D A ( Ir) n D A {■ Il,Ir ) (68). 


Thus, one can conclude from (154) and S (/) (T Dh ( I ) C 

2 )a {IljIr) that the disks starting at almost any configuration 
in the positively invariant D(3,b) reach D(3',b l ) in finite 
time, and this completes the proof. ■ 
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